AllinToyou

这个屌丝很懒，什么也没留下！

热门标签

A*算法初学者贴及python程序详解_a算法python实现

作者：AllinToyou | 2024-05-02 21:14:47

踩

a算法python实现

提示：本人是第一次学习A*算法，记录自己的学习过程，捋清其原理步骤，并利用python做一个小例子实现。

A*算法的基本原理

将从应用场景、思想、基本的定义进行说明

A*算法的应用场景

一副地图中有坐标A和B，而A和B之间可能存在一些障碍，需要找到一条路径从A到B尽可能最短的安全路径。这样的问题就称作路径规划问题。
A*算法是处理路径规划问题的一种算法，其他的还有Dijkstra算法、贪心算法、广度优先（Bread First Search，BFS）和深度优先算法（Depth First Search，DFS）。BFS的思想就是以起点A为圆心，先搜索A周围的所有点，形成一个类似圆的搜索区域，再扩大搜索半径，直到终点B进入搜索区域内被找到；DFS的思想就是从起点A出发找沿着AB这条线的方向，并且离A点最远的点。
研究表明，A-star算法搜索效率较高。

A*算法的思想

A*算法利用估价的思想，其规划的过程如下：

在代遍历列表中（刚开始只有A点），我们在列表中查找一个估价（见估价公式）最小的点K
对点K进行一次广度优先搜索，也就是它移动到一次到的下一个坐标（上下左右、左上右上、左下右下）不包含已经遍历过的点和不能到达的点，将能查找的点添加到队列中，并将点K从队列中移除
重复1、2步骤直到到达B点，或者队列已经为空，表示没有路径可以到达点B

估价公式
F = G + H
G = 从起点A移动到下一移动位置的代价，沿着到达该位置而生成的路径。约定直行移动一次的代价是10，对角线的移动代价为14。
H = 从指定的位置移动到终点B的估价。计算从当前位置横向或者纵向移动到达终点所经过的距离（距离的计算见下面）乘10，忽略对角移动。

A*算法的定义

open list : 记录下所有被考虑来寻找最短路径的格子
close list : 记录下不会再被考虑的格子
Point（属性） : 是否为障碍物，父节点

用来计算代价的距离：欧拉距离和曼哈顿距离

欧拉距离：
(x₁-x₂)²+ (y₁-y₂)²
曼哈顿距离：
|x₁-x₂|+ |y₁-y₂|

A*算法的路径规划步骤

首先定义一张定宽高的地图（定义好Point点的地图，其中有Is_Wall属性，属性标记是否有障碍物）
设定好起始点和终点（起始点A，终点为B）
开始寻路
① 从起点A开始，把A作为一个等待检查的方格，放入到open list中（open list存放每一个等待检查的方格）
② 寻找起点A周围可以到达的方格（最多八个），将它们放入到open list，并设置它们的父节点为A
③ 从open list中删除起点A，并将A放入到close list中（close list存放的是不再需要检查的方格）
④ 计算open list中的方格的代价值（F）
⑤ 从open list中选择F最小的方格K，如果F一样，那么选择唯一一个G值最小的方格K，将其从open list中删除，放入close list中
⑥ 检查K所有邻近并且可达的方格
a）障碍物和close list中的方格不考虑
b）如果这些方格还不在open list里面，就将它们加入到open list，并且计算这些方格的F值，并设置父节点为K
c）如果K相邻的方格C已经在open list中，计算新的路径从A到方格C（即经过K的路径），判断是否需要更新：G值是否更低一点
如果新的G值更低，则修改父节点为方格K，重新计算F值，H值不需要改变，因为方格到达目标点的预计消耗是固定的。
如果新的G值比较高，则说明新的路径消耗更高，则值不做改变（G值不变也不更新）
⑦ 继续从open list中找出F值最小的，从open list中删除，添加到close list，再继续找出周围可以到达的方块，如此循环。
⑧ 结束判断：当open list中出现终点B时，说明路径已经找到；当open list中没有了数据，则说明没有合适路径。

A*算法路径搜索的图示演示

每一个方格的表示如下：
在这里插入图片描述
左上角代表F，即代价值；左下角代表G，右下角代表H，箭头指向当前节点的父节点
距离采用曼哈顿距离。

计算邻近点的代价，它们的父节点都是起点，找出最小估价，为右上角的方块。可以规定估价计算的顺序。
对上个步骤选出来的右上角的方块（紫色方块）的八个方向的坐标进行估值（已经估值的以及障碍不用计算），G值最小最后访问的点为父节点。
对上个步骤的紫色方块放入close list，即深绿色方块，重新寻找F值最小的方块。
重复上述步骤
循环结束
循环结束的条件就是，终点在open list中，即找到了终点或者open list中没有了值，这就说明没有找到路径，由此终点依次查找它们的父节点直到起点，将坐标点逆序，就是最短安全路径。

A*算法的代码举例

在了解了以上的原理与步骤后，开始组织程序

1.定义一张定宽高的地图并设定起点和终点

这个步骤要求定义一个定宽高的地图，并且在地图中要标明障碍物，利用一个二维数组来定义，并且需要定义是否有障碍
根据上面分析用的图，可以发现是一个10×10的方形区域
规定：

数字	0	1	3	4	6
含义	障碍物	可以通行	open list	close list	最后的路径

代码如下：

#coding=utf-8
import numpy as np
import pandas as pd
import matplotlib as mpl
import matplotlib.pyplot as plt

map_grid=[[1 for j in range(0,10)] for i in range(0,10)]  #定义列表
map_grid = np.array(map_grid)  #将列表转化为数组，因为只有数组才有维度的概念，方便切片
map_grid[4:8,4]=0   #障碍物
map_grid[2:4,6]=0
map_grid[4,2]=3     #起点
map_grid[6,8]=6     #终点
plt.imshow(map_grid,cmap=plt.cm.hot,interpolation='nearest',vmin=0,vmax=10)  #绘制热力图
plt.colorbar()
plt.xlim(-1,10)     #x轴的范围
plt.ylim(-1,10)
my_x_ticks = np.arange(0,11,1)   #x轴标号的范围
my_y_ticks = np.arange(0,11,1)
plt.xticks(my_x_ticks)
plt.yticks(my_y_ticks)
plt.grid(True)   #开启栅格  可以不开启
plt.show()  #可视化
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

效果：
与上述分析的地图一致
在这里插入图片描述
将其打包成一个函数

def draw_effect(map_grid):
    plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10)  # 绘制热力图
    plt.colorbar()
    plt.xlim(-1, 10)  # x轴的范围
    plt.ylim(-1, 10)
    my_x_ticks = np.arange(0, 11, 1)  # x轴标号的范围
    my_y_ticks = np.arange(0, 11, 1)
    plt.xticks(my_x_ticks)
    plt.yticks(my_y_ticks)
    plt.grid(True)  # 开启栅格  可以不开启
    plt.show()  # 可视化
1
2
3
4
5
6
7
8
9
10
11

2.开始路径规划

在这个部分需要设置两个列表open_list，close_list，并且不断更新上面定义的二维数组中的值
①从起点A开始，把A作为一个等待检查的方格，放入open list中（open list存放每一个等待检查的方格）

open_list = [4,2]
G = 0
H = (abs(xn - x0)+abs(yn - y0))*10
F = G+H
open_list = {"x":4,"y":2,"Fvalue":F,"Gvalue":G,"Hvalue":H,"f_x":None,"f_y":None}
open_list = pd.DataFrame(open_list,index=[0])
#分别代表x,y,F值,G值,H值,父节点x,父节点y
1
2
3
4
5
6
7

②寻找起点A周围可以到达的方格，放入open_list中，并设置父节点，并计算F、G、H的值
，将此寻周围方格的过程打包为一个函数：如果节点K的邻节点在open_list，如果新的G值更小就更新G值，并重新计算F值，并更改K的父节点；否则不变

#②寻找起点A周围可以到达的方格，放入openlist中,并设置其父节点为A，并计算出F值(包装成一个函数来实现)
#close_list中不搜索，障碍不搜索，已经搜索过(在openlist里面)的不再搜索
def find_way(open_list,close_list,x0,y0):
    #定义一个搜索顺序，右->右上->上->左上->左->左下->下->右下
    m = close_list.iloc[close_list.index[(close_list['x'] == x0) & (close_list['y'] == y0)], 3].values.astype(int)
    if y0 + 1 < 10 and map_grid[x0, y0 + 1] != 0 and map_grid[x0, y0 + 1] !=4:
        x1 = x0
        y1 = y0 + 1
        G = 10 + m
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 + 1 < 10 and x0 + 1 < 10 and map_grid[x0 + 1, y0 + 1] != 0 and map_grid[x0+1, y0 + 1] !=4:
        G = 14 + m
        x1 = x0+1
        y1 = y0 + 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

        #pen_list.loc[len(open_list.index)] = [x1, y1, F, G, H, x0, y0]


    if x0 + 1 < 10 and map_grid[x0 + 1, y0] != 0 and map_grid[x0 + 1, y0] !=4:
        G = 10 + m
        x1 = x0+1
        y1 = y0
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if x0 + 1 < 10 and y0 -1 >=0 and map_grid[x0 + 1, y0 - 1] != 0 and map_grid[x0 + 1, y0 - 1] !=4:
        G = 14 + m
        x1 = x0+1
        y1 = y0-1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 - 1 >= 0 and map_grid[x0, y0 - 1] != 0 and map_grid[x0, y0 - 1] !=4:
        G = 10 + m
        x1 = x0
        y1 = y0 - 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 - 1 >= 0 and x0 - 1 >= 0  and map_grid[x0 - 1, y0 - 1] != 0 and map_grid[x0 - 1, y0 - 1] !=4:
        G = 14 + m
        x1 = x0-1
        y1 = y0 - 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if x0 - 1 >= 0 and map_grid[x0 - 1, y0] != 0 and map_grid[x0 - 1, y0] !=4:
        G = 10 + m
        x1 = x0-1
        y1 = y0
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 + 1 < 10 and x0 - 1 >=0 and map_grid[x0 - 1, y0 + 1] != 0 and map_grid[x0 - 1, y0 + 1] !=4:
        G = 14 + m
        x1 = x0-1
        y1 = y0 + 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    return open_list
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201

③从open_list中删除起点A，并将A放入close_list中

#③从open list中删除起点A，并将A放入close list中(close list存放不需要再检查的方格)
insertRow = open_list.iloc[open_list.index[(open_list['x'] == x0) & (open_list['y'] == y0)]]
close_list = insertRow
close_list = pd.DataFrame(close_list)
# print(open_list)                        #此时openlist里面就有所有可以到达的方格
open_list = open_list.drop(open_list.index[(open_list['x'] == x0) & (open_list['y'] == y0)])  # 删除起点A
open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
map_grid[x0, y0] = 4  # close_list

open_list = find_way(open_list,close_list,x0,y0)                #调用该函数
open_list = pd.DataFrame(open_list)
1
2
3
4
5
6
7
8
9
10
11

④从open_list中选择F最小的方格K，如果F最小的方格不只一个，那么优先搜索G比较大的方格，将其从open_list中删除，放入close_list，如果G一样大，那么就只是搜索列表中的第一个。

#④从open_list中选择F最小的方格K，如果F最小的方格不只一个，那么优先搜索G比较大的方格，将其从open_list中删除，放入close_list
def move_min_to_close(open_list,close_list,x0,y0):
    insertRow = open_list.loc[open_list.index[(open_list['Fvalue'] == open_list['Fvalue'].min())]]
    insertRow = insertRow.loc[insertRow.index[(insertRow['Gvalue'] == insertRow['Gvalue'].min())]]
    insertRow = insertRow.loc[[insertRow.index[0]]]
    open_list = open_list.drop(insertRow.index)  # 删除起点K
    open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
    close_list = pd.concat([close_list, insertRow])
    close_list = close_list.reset_index(drop=True)  # 重新排列index(索引)
    insertRow = insertRow.reset_index(drop=True)  # 重新排列index(索引)
    x0 = insertRow.iloc[0, 0]
    y0 = insertRow.iloc[0, 1]
    return [open_list,close_list,x0,y0]
1
2
3
4
5
6
7
8
9
10
11
12
13

⑤循环上述步骤，找出F最小的，找出周围可以到达的方块，从open list中删除，添加到close list；
循环结束的条件:当open list中出现终点B时，说明路径已经找到；当open list中没有了数据，则说明没有合适路径

#⑤循环上述步骤，找出F最小的，找出周围可以到达的方块，从open list中删除，添加到close list
#循环结束的条件:当open list中出现终点B时，说明路径已经找到；当open list中没有了数据，则说明没有合适路径
while (0 not in open_list['Hvalue'].values.astype(int) and pd.isnull(open_list.loc[0,'x'])==False):

    [open_list, close_list,x0,y0] = move_min_to_close(open_list, close_list, x0, y0)
    map_grid[x0, y0] = 4  # close_list
    open_list = pd.DataFrame(open_list)
    close_list = pd.DataFrame(close_list)
    open_list = find_way(open_list, close_list, x0, y0)
    #print(x0,y0)
1
2
3
4
5
6
7
8
9
10

⑥总结路径，并绘制图
当open list中出现终点B时，说明路径已经找到，绘制热力图，并打印路径节点；
当open list中没有了数据，则说明没有合适路径，绘制热力图，并发送‘No way’

if 0 in open_list['Hvalue'].values.astype(int):
    print("find it")
    list = [[xn,yn]]
    [x,y] = open_list.loc[((open_list['x'].values == xn) & (open_list['y'].values == yn)), ['f_x','f_y']].values.astype(int)[0]
    list.append([x,y])
    map_grid[x, y] = 6  # 终点
    while close_list.loc[((close_list['x'].values == x) & (close_list['y'].values == y)), 'f_x'].values[0]!=None:
        [x,y] = close_list.loc[((close_list['x'].values == x) & (close_list['y'].values == y)), ['f_x','f_y']].values.astype(int)[0]
        map_grid[x, y] = 6  # 终点
        list.append([x,y])
    draw_effect(map_grid)
    print(list)

if(pd.isnull(open_list.loc[0,'x'])==True):
    print("No way")
    draw_effect(map_grid)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

效果图：
在这里插入图片描述

总程序附录

#coding=utf-8
import numpy as np
import pandas as pd
import matplotlib as mpl
import matplotlib.pyplot as plt
import warnings

warnings.filterwarnings("ignore")

#1.建立一个地图，明确障碍物和能够同行的路和起点、终点
map_grid=[[1 for j in range(0,10)] for i in range(0,10)]  #定义列表
map_grid = np.array(map_grid)  #将列表转化为数组，因为只有数组才有维度的概念，方便切片
map_grid[4:8,4]=0   #障碍物
map_grid[2:4,6]=0
map_grid[4,2]=1     #起点
map_grid[6,8]=6     #终点
x0 = 4
y0 = 2
xn = 6
yn = 8
i=0
#建图完成，其中map_grid里面就是每个方格的属性
def draw_effect(map_grid):
    plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10)  # 绘制热力图
    plt.colorbar()
    plt.xlim(-1, 10)  # x轴的范围
    plt.ylim(-1, 10)
    my_x_ticks = np.arange(0, 11, 1)  # x轴标号的范围
    my_y_ticks = np.arange(0, 11, 1)
    plt.xticks(my_x_ticks)
    plt.yticks(my_y_ticks)
    plt.grid(True)  # 开启栅格  可以不开启
    plt.show()  # 可视化


#2.开始寻路

#①从起点A开始，把A作为一个等待检查的方格，放入open list中（存放每一个等待检查的方格）
open_list = [4,2]
G = 0
H = (abs(xn - x0)+abs(yn - y0))*10
F = G+H
open_list = {"x":4,"y":2,"Fvalue":F,"Gvalue":G,"Hvalue":H,"f_x":None,"f_y":None}
open_list = pd.DataFrame(open_list,index=[0])
#分别代表x,y,F值,G值,H值,父节点x,父节点y
#print(open_list.iloc[open_list.index[(open_list['x'] == x0) & (open_list['y'] == y0)], 3])


#②寻找起点A周围可以到达的方格，放入openlist中,并设置其父节点为A，并计算出F值(包装成一个函数来实现)
#close_list中不搜索，障碍不搜索，已经搜索过(在openlist里面)的不再搜索
def find_way(open_list,close_list,x0,y0):
    #定义一个搜索顺序，右->右上->上->左上->左->左下->下->右下
    m = close_list.iloc[close_list.index[(close_list['x'] == x0) & (close_list['y'] == y0)], 3].values.astype(int)
    if y0 + 1 < 10 and map_grid[x0, y0 + 1] != 0 and map_grid[x0, y0 + 1] !=4:
        x1 = x0
        y1 = y0 + 1
        G = 10 + m
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 + 1 < 10 and x0 + 1 < 10 and map_grid[x0 + 1, y0 + 1] != 0 and map_grid[x0+1, y0 + 1] !=4:
        G = 14 + m
        x1 = x0+1
        y1 = y0 + 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

        #pen_list.loc[len(open_list.index)] = [x1, y1, F, G, H, x0, y0]


    if x0 + 1 < 10 and map_grid[x0 + 1, y0] != 0 and map_grid[x0 + 1, y0] !=4:
        G = 10 + m
        x1 = x0+1
        y1 = y0
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if x0 + 1 < 10 and y0 -1 >=0 and map_grid[x0 + 1, y0 - 1] != 0 and map_grid[x0 + 1, y0 - 1] !=4:
        G = 14 + m
        x1 = x0+1
        y1 = y0-1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 - 1 >= 0 and map_grid[x0, y0 - 1] != 0 and map_grid[x0, y0 - 1] !=4:
        G = 10 + m
        x1 = x0
        y1 = y0 - 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 - 1 >= 0 and x0 - 1 >= 0  and map_grid[x0 - 1, y0 - 1] != 0 and map_grid[x0 - 1, y0 - 1] !=4:
        G = 14 + m
        x1 = x0-1
        y1 = y0 - 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if x0 - 1 >= 0 and map_grid[x0 - 1, y0] != 0 and map_grid[x0 - 1, y0] !=4:
        G = 10 + m
        x1 = x0-1
        y1 = y0
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 + 1 < 10 and x0 - 1 >=0 and map_grid[x0 - 1, y0 + 1] != 0 and map_grid[x0 - 1, y0 + 1] !=4:
        G = 14 + m
        x1 = x0-1
        y1 = y0 + 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    return open_list


#③从open list中删除起点A，并将A放入close list中(close list存放不需要再检查的方格)
insertRow = open_list.iloc[open_list.index[(open_list['x'] == x0) & (open_list['y'] == y0)]]
close_list = insertRow
close_list = pd.DataFrame(close_list)
# print(open_list)                        #此时openlist里面就有所有可以到达的方格
open_list = open_list.drop(open_list.index[(open_list['x'] == x0) & (open_list['y'] == y0)])  # 删除起点A
open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
map_grid[x0, y0] = 4  # close_list

open_list = find_way(open_list,close_list,x0,y0)                #调用该函数
open_list = pd.DataFrame(open_list)


#④从open_list中选择F最小的方格K，如果F最小的方格不只一个，那么优先搜索G比较大的方格，将其从open_list中删除，放入close_list
def move_min_to_close(open_list,close_list,x0,y0):
    insertRow = open_list.loc[open_list.index[(open_list['Fvalue'] == open_list['Fvalue'].min())]]
    insertRow = insertRow.loc[insertRow.index[(insertRow['Gvalue'] == insertRow['Gvalue'].min())]]
    insertRow = insertRow.loc[[insertRow.index[0]]]
    open_list = open_list.drop(insertRow.index)  # 删除起点K
    open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
    close_list = pd.concat([close_list, insertRow])
    close_list = close_list.reset_index(drop=True)  # 重新排列index(索引)
    insertRow = insertRow.reset_index(drop=True)  # 重新排列index(索引)
    x0 = insertRow.iloc[0, 0]
    y0 = insertRow.iloc[0, 1]
    return [open_list,close_list,x0,y0]

#print(open_list)
#⑤循环上述步骤，找出F最小的，找出周围可以到达的方块，从open list中删除，添加到close list
#循环结束的条件:当open list中出现终点B时，说明路径已经找到；当open list中没有了数据，则说明没有合适路径
while (0 not in open_list['Hvalue'].values.astype(int) and pd.isnull(open_list.loc[0,'x'])==False):

    [open_list, close_list,x0,y0] = move_min_to_close(open_list, close_list, x0, y0)
    map_grid[x0, y0] = 4  # close_list
    open_list = pd.DataFrame(open_list)
    close_list = pd.DataFrame(close_list)
    open_list = find_way(open_list, close_list, x0, y0)
    #print(x0,y0)

if 0 in open_list['Hvalue'].values.astype(int):
    print("find it")
    list = [[xn,yn]]
    [x,y] = open_list.loc[((open_list['x'].values == xn) & (open_list['y'].values == yn)), ['f_x','f_y']].values.astype(int)[0]
    list.append([x,y])
    map_grid[x, y] = 6  # 终点
    while close_list.loc[((close_list['x'].values == x) & (close_list['y'].values == y)), 'f_x'].values[0]!=None:
        [x,y] = close_list.loc[((close_list['x'].values == x) & (close_list['y'].values == y)), ['f_x','f_y']].values.astype(int)[0]
        map_grid[x, y] = 6  # 终点
        list.append([x,y])
    draw_effect(map_grid)
    print(list)

if(pd.isnull(open_list.loc[0,'x'])==True):
    print("No way")
    draw_effect(map_grid)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306

总结

程序是自己一句一句写的，没有参考别人的程序，看着有点复杂，其实只要花时间读一下代码，很容易理解，适合初学者。但总算把A*捋清楚了，也学会了python里面list，dataframe，array的索引和用法。
接下来准备写dijkstra算法。

参考文献：

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/AllinToyou/article/detail/526130