A*算法原理与实现（python）_python a*

A*算法概述

A*算法是路径规划表现较好的算法之一。所谓路规划是，从A点到B点找到一条合适的路径，使得智能体完成从A到B的运动。所谓合适，是指智能体定义的参数指标，如步数最少，代价最低等等。不同的应用场景具有不同的参数指标。路径规划广泛应用于智能体寻路、八数码难题游戏等场景。

A*算法原理

在路径规划中，由于可能有障碍存在，使得路径上的一些点不可用。在路径规划中，需要避开这些点。在寻找合理路径中，如何评价路径呢。在A*算法中，通常采用
$$F=G+H$$

来评估该路径。其中G是从起始点A到指定点的移动代价。H是指定点到终点B的代价。其中G是可以通过计算获得的，而H不可以准确获得，其原因在于在指定点到终点B的路径上我们并不知道障碍物是怎么样的，因此H是一种估计，通常被称之为试探法。这里我们使用 Manhattan 方法，计算从当前方格横向或纵向移动到达目标所经过的方格数，忽略对角移动，然后把总数乘以 10 。之所以叫做 Manhattan 方法，是因为这很像统计从一个地点到另一个地点所穿过的街区数，而你不能斜向穿过街区。重要的是，计算 H 是，要忽略路径中的障碍物。这是对剩余距离的估算值，而不是实际值，因此才称为试探法。当H为0时，A*算法退化成迪杰斯特拉算法。

移动代价可以通过距离公式来计算，距离的计算有多种维度，包括曼哈顿距离、欧拉距离等等，应根据具体的场景选取合适的计算方式。

在使用A算法进行路径规划时，有两个重要的列表将被使用，那就是openlist和closelist。openlist存放是路径可能存放的点，即openlist是个待检查的列表。而closelist是不在需要关注的点，即某点被openlist移除，就会被放到closelist。
A的算法步骤如下：
1、把起点加入到openlist
2、然后重复下面过程
*.遍历openlist，找到路径评价F值最小的节点，把它当做当前要处理的节点。
*.把该节点从openlist移到closelist
*.对该节点的所有邻居节点做如下处理
**.如果不在openlist里，则加入openlist，并把当前节点设置为父节点，记录该邻居节的F,G,H值
**.如果在openlist，如果当前节点到该邻居节点的路径代价小于当前节点的路径代价，则用更小的值来代替该 邻居节点的G，同时把当前节点设置为邻居节点的父节点。
3、当把终点加到openlist时候，证明路径找到，并沿着父节点可以找到路径，否则查找终点失败，并且openlist是空的，此时没有路径

A*算法代码实现示例

一个代码示例如下：

# -*- coding: utf-8 -*-
# @Time    : 2019/12/2 22:37
# @Author  : HelloWorld！
# @FileName: A_Star.py
# @Software: PyCharm
# @Operating System: Windows 10
# @Python.version: 3.5


class Array2D:
    """

        说明：

            1.构造方法需要两个参数，即二维数组的宽和高

            2.成员变量w和h是二维数组的宽和高

            3.使用：‘对象[x][y]’可以直接取到相应的值

            4.数组的默认值都是0

    """

    def __init__(self, w, h):

        self.w = w

        self.h = h

        self.data = []

        self.data = [[0 for y in range(h)] for x in range(w)]

    def showArray2D(self):

        for y in range(self.h):

            for x in range(self.w):
                print(self.data[x][y], end=' ')

            print("")

    def __getitem__(self, item):

        return self.data[item]


class Point:
    """

    表示一个点

    """

    def __init__(self, x, y):
        self.x = x;
        self.y = y

    def __eq__(self, other):
        if self.x == other.x and self.y == other.y:
            return True

        return False

    def __str__(self):
        return "x:" + str(self.x) + ",y:" + str(self.y)


class AStar:
    """

    AStar算法的Python3.x实现

    """

    class Node:  # 描述AStar算法中的节点数据

        def __init__(self, point, endPoint, g=0):
            self.point = point  # 自己的坐标

            self.father = None  # 父节点

            self.g = g  # g值，g值在用到的时候会重新算

            self.h = (abs(endPoint.x - point.x) + abs(endPoint.y - point.y)) * 10  # 计算h值

    def __init__(self, map2d, startPoint, endPoint, passTag=0):

        """

        构造AStar算法的启动条件

        :param map2d: Array2D类型的寻路数组

        :param startPoint: Point或二元组类型的寻路起点

        :param endPoint: Point或二元组类型的寻路终点

        :param passTag: int类型的可行走标记（若地图数据!=passTag即为障碍）

        """

        # 开启表

        self.openList = []

        # 关闭表

        self.closeList = []

        # 寻路地图

        self.map2d = map2d

        # 起点终点

        if isinstance(startPoint, Point) and isinstance(endPoint, Point):

            self.startPoint = startPoint

            self.endPoint = endPoint

        else:

            self.startPoint = Point(*startPoint)

            self.endPoint = Point(*endPoint)

        # 可行走标记

        self.passTag = passTag

    def getMinNode(self):

        """

        获得openlist中F值最小的节点

        :return: Node

        """

        currentNode = self.openList[0]

        for node in self.openList:

            if node.g + node.h < currentNode.g + currentNode.h:
                currentNode = node

        return currentNode

    def pointInCloseList(self, point):

        for node in self.closeList:

            if node.point == point:
                return True

        return False

    def pointInOpenList(self, point):

        for node in self.openList:

            if node.point == point:
                return node

        return None

    def endPointInCloseList(self):

        for node in self.openList:

            if node.point == self.endPoint:
                return node

        return None

    def searchNear(self, minF, offsetX, offsetY):

        """

        搜索节点周围的点

        :param minF:F值最小的节点

        :param offsetX:坐标偏移量

        :param offsetY:

        :return:

        """

        # 越界检测

        if minF.point.x + offsetX < 0 or minF.point.x + offsetX > self.map2d.w - 1 or minF.point.y + offsetY < 0 or minF.point.y + offsetY > self.map2d.h - 1:
            return

        # 如果是障碍，就忽略

        if self.map2d[minF.point.x + offsetX][minF.point.y + offsetY] != self.passTag:
            return

        # 如果在关闭表中，就忽略

        currentPoint = Point(minF.point.x + offsetX, minF.point.y + offsetY)

        if self.pointInCloseList(currentPoint):
            return

        # 设置单位花费

        if offsetX == 0 or offsetY == 0:

            step = 10

        else:

            step = 14

        # 如果不再openList中，就把它加入openlist

        currentNode = self.pointInOpenList(currentPoint)

        if not currentNode:
            currentNode = AStar.Node(currentPoint, self.endPoint, g=minF.g + step)

            currentNode.father = minF

            self.openList.append(currentNode)

            return

        # 如果在openList中，判断minF到当前点的G是否更小

        if minF.g + step < currentNode.g:  # 如果更小，就重新计算g值，并且改变father

            currentNode.g = minF.g + step

            currentNode.father = minF

    def start(self):

        """

        开始寻路

        :return: None或Point列表（路径）

        """

        # 判断寻路终点是否是障碍

        if self.map2d[self.endPoint.x][self.endPoint.y] != self.passTag:
            return None

        # 1.将起点放入开启列表

        startNode = AStar.Node(self.startPoint, self.endPoint)

        self.openList.append(startNode)

        # 2.主循环逻辑

        while True:

            # 找到F值最小的点

            minF = self.getMinNode()

            # 把这个点加入closeList中，并且在openList中删除它

            self.closeList.append(minF)

            self.openList.remove(minF)

            # 判断这个节点的上下左右节点

            self.searchNear(minF, 0, -1)

            self.searchNear(minF, 0, 1)

            self.searchNear(minF, -1, 0)

            self.searchNear(minF, 1, 0)

            # 判断是否终止

            point = self.endPointInCloseList()

            if point:  # 如果终点在关闭表中，就返回结果

                # print("关闭表中")

                cPoint = point

                pathList = []

                while True:

                    if cPoint.father:

                        pathList.append(cPoint.point)

                        cPoint = cPoint.father

                    else:

                        # print(pathList)

                        # print(list(reversed(pathList)))

                        # print(pathList.reverse())

                        return list(reversed(pathList))

            if len(self.openList) == 0:
                return None

if __name__ == '__main__':

    #创建一个10*10的地图

    map2d=Array2D(10,10)

    #设置障碍

    map2d[4][0]= 1

    map2d[4][1] = 1

    map2d[4][2] = 0

    map2d[4][3] = 1

    map2d[4][4] = 1

    map2d[4][5] = 1

    map2d[4][6] = 1

    #显示地图当前样子

    map2d.showArray2D()

    #创建AStar对象,并设置起点为0,0终点为9,0

    aStar=AStar(map2d,Point(0,0),Point(9,0))

    #开始寻路

    pathList=aStar.start()

    #遍历路径点,在map2d上以'8'显示

    for point in pathList:

        map2d[point.x][point.y]=8

        # print(point)

    print("----------------------")

    #再次显示地图

    map2d.showArray2D()

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
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369

结果如下：

0 0 0 0 1 0 0 0 0 0 
0 0 0 0 1 0 0 0 0 0 
0 0 0 0 0 0 0 0 0 0 
0 0 0 0 1 0 0 0 0 0 
0 0 0 0 1 0 0 0 0 0 
0 0 0 0 1 0 0 0 0 0 
0 0 0 0 1 0 0 0 0 0 
0 0 0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 0 0 
----------------------
0 8 8 8 1 8 8 8 8 8 
0 0 0 8 1 8 0 0 0 0 
0 0 0 8 8 8 0 0 0 0 
0 0 0 0 1 0 0 0 0 0 
0 0 0 0 1 0 0 0 0 0 
0 0 0 0 1 0 0 0 0 0 
0 0 0 0 1 0 0 0 0 0 
0 0 0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 0 0 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

参考文献
https://blog.csdn.net/qq_39687901/article/details/88554716
https://blog.csdn.net/ctwy291314/article/details/95055008
https://blog.csdn.net/qq_39687901/article/details/80753433