python -A*算法_a*算法python

前言

A算法是一种静态网中最短路径最有效的直接搜索方法。多用于游戏和物流中的路径规划问题。
A算法的详细解读可以看iwiniwin —http://www.cnblogs.com/iwiniwin/，这个博主写的很不错

一、A*算法实现步骤

A*算法的大致过程：
1.将初始节点放入到open列表中。
2.判断open列表。如果为空，则搜索失败。如果open列表中存在目标节点，则搜索成功。
3.从open列表中取出F值最小的节点作为当前节点，并将其加入到close列表中。
4.计算当前节点的相邻的所有可到达节点，生成一组子节点。对于每一个子节点：
-如果该节点在close列表中，则丢弃它
-如果该节点在open列表中，则检查其通过当前节点计算得到的F值是否更小，如果更小则更新其F值，并将其父节点设置为当前节点。
-如果该节点不在open列表中，则将其加入到open列表，并计算F值，设置其父节点为当前节点。
转到2步骤

二、python代码

1.，地图及移动成本

地图是栅格地图，以二维数组的形式展开，每一个数代表一个节点（道路），数的取值代表节点（道路）信息，其中0表示道路通畅，-1表示移动对象（起点），1表示道路阻塞，2表示终点。
行动成本是前进一格需要花费的成本，在这里将水平或垂直移动一格的成本设置为10，对角移动一个的成本设置为14。

# 地图，栅格状地图，二维数组,每个节点有两个状态，可通过0与不可通过1
maps = np.array([
                [0, 1, 0, 0, 0, 0],
                [-1, 0, 0, 1, 0, 1],
                [0, 0, 1, 1, 0, 1],
                [1, 0, 1, 0, 1, 1],
                [0, 0, 1, 0, 1, 0],
                [1, 0, 1, 0, 0, 2]
                ], dtype=int)
    #上北下南
##移动 ，水平或垂直的移动成本是10，对角的移动成本是14
move_long10 = 10
move_long14 = 14
1
2
3
4
5
6
7
8
9
10
11
12
13

2.设置列表等数据

def __init_start(self):
     self.end = True
     self.pos = {  # 起点，终点，地图大小
         "start_w": 0,
         "start_h": 0,
         "end_w": 0,
         "end_h": 0,
         'threshold_we': 0,
         'threshold_sn': 0
     }
     # 储存路径数据，节点信息（节点坐标，节点方向，节点数目），路径总长度
     # [[父节点坐标], [当前点坐标], f(路径计分), g(从起点到网格上给定正方形的移动成本，遵循到达那里的路径。), h(最短距离), string(节点信息) ]
     self.parent_map = {
         "parent": {'w': 0, 'h': 0},
         "son": {'w': 0, 'h': 0},
         "f": 0,
         "g": 0,
         "h": 0,
         "str": 0}
     # 坐标信息
     self.map_coor = {
         'coor_pos': {'w': 0, 'h': 0},
         'f': 0,
         'g': 0,
         'h': 0,
         'walkable': 0
     }
     self.open_lists = []
     self.close_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

3.设置子节点

每次获取子节点的几个方向是动态的，会随障碍物或者边缘，删除或添加某个方向的节点
（对角暂时未用），相信你，看完代码，上手实践后，就会用了

    def __childer(self):
    	#上北下南
        # # 每个节点有8个方向，（边缘节点除外）下，左，上，右，，，，
        # # East东, south南, west西, north北, southeast东南, northeast东北, southwest西南, northwest西北
        # # 子节点
        Childer = {
            #'wn': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
            'n': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
            #'en': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
            'w': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
            'e': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
            #'ws': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
            's': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
            #'es': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0}
        }
        return Childer
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

4.初始化起点和终点坐标及地图大小

    def __init_pos(self, map_array, start_str, end_str):
        start_pos = np.argwhere(map_array == start_str)
        end_pos = np.argwhere(map_array == end_str)
        threshold = np.shape(map_array)
        self.pos = {
            "start_h": start_pos[0][0],
            "start_w": start_pos[0][1],
            "end_h": end_pos[0][0],
            "end_w": end_pos[0][1],
            'threshold_we': threshold[0],
            'threshold_sn': threshold[1]
        }
1
2
3
4
5
6
7
8
9
10
11
12

5.初始化父坐标

#初始化父节点
    def __init_parent_map(self):
        g = 0
        h = self.move_long10*(abs(self.pos['start_w']-self.pos['end_w'])+abs(self.pos['start_h']-self.pos['end_h']))
        self.parent_map = {
                "parent": {'w': self.pos['start_w'], 'h': self.pos['start_h']},
                "son": {'w': self.pos['start_w'], 'h': self.pos['start_h']},
                "f": g+h,
                "g": g,
                "h": h,
                "str": -1}
1
2
3
4
5
6
7
8
9
10
11

6.将起点加入到open列表中

#起点加入open list
        self.map_coor = {
            'coor_pos': {'w': self.pos['start_w'], 'h': self.pos['start_h']},
            'f': self.parent_map['f'],
            'g': self.parent_map['g'],
            'h': self.parent_map['h'],
            'walkable': 0
        }
        self.open_lists.append(self.parent_map)
1
2
3
4
5
6
7
8
9

7.从open列表最小F节点，存入close列表中

    def __find_open_fmin(self):
        f = 65536
        w_h = None
        for i in range(len(self.open_lists)):
            if self.open_lists[i]['str'] != 1:
                if self.open_lists[i]['f'] < f:
                    f = self.open_lists[i]['f']
                    w_h = i
        self.close_list.append(self.open_lists[w_h])
        self.open_lists.pop(w_h)
1
2
3
4
5
6
7
8
9
10

8. 获取子节点

获取该节点的子节点

 # # East东, south南, west西, north北, southeast东南, northeast东北, southwest西南, northwest西北
    # def fgh_g(self, ):
    def __getchild_one(self, child, parent_pos, pos, g):
        for i in child:
            if i == 'wn':
                if child[i]['x'] == 0 and child[i]['y'] == 0:
                    child[i]['x'] = parent_pos['son']['w'] - 1 if parent_pos['son']['w'] - 1 >= 0 else -1
                    child[i]['y'] = parent_pos['son']['h'] - 1 if parent_pos['son']['h'] - 1 >= 0 else -1
                    child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
                        child[i]['y'] - pos['end_h']))*self.move_long10
                    child[i]['g'] = g + 14
                    child[i]['f'] = child[i]['h'] + child[i]['g']
            elif i == 'n':
                if child[i]['x'] == 0 and child[i]['y'] == 0:
                    child[i]['x'] = parent_pos['son']['w']
                    child[i]['y'] = parent_pos['son']['h'] - 1 if parent_pos['son']['h'] - 1 >= 0 else -1
                    child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
                        child[i]['y'] - pos['end_h'])) * self.move_long10
                    child[i]['g'] = g + 10
                    child[i]['f'] = child[i]['h'] + child[i]['g']
            elif i == 'en':
                if child[i]['x'] == 0 and child[i]['y'] == 0:
                    child[i]['x'] = parent_pos['son']['w'] + 1 if parent_pos['son']['w'] + 1 < pos["threshold_we"] else -1
                    child[i]['y'] = parent_pos['son']['h'] - 1 if parent_pos['son']['h'] - 1 >= 0 else -1
                    child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
                        child[i]['y'] - pos['end_h'])) * self.move_long10
                    child[i]['g'] = g + 14
                    child[i]['f'] = child[i]['h'] + child[i]['g']
            elif i == 'w':
                if child[i]['x'] == 0 and child[i]['y'] == 0:
                    child[i]['x'] = parent_pos['son']['w'] - 1 if parent_pos['son']['w'] - 1 >= 0 else -1
                    child[i]['y'] = parent_pos['son']['h']
                    child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
                        child[i]['y'] - pos['end_h'])) * self.move_long10
                    child[i]['g'] = g + 10
                    child[i]['f'] = child[i]['h'] + child['n']['g']

            elif i == 'e':
                if child[i]['x'] == 0 and child[i]['y'] == 0:
                    child[i]['x'] = parent_pos['son']['w'] + 1 if parent_pos['son']['w'] + 1 < pos["threshold_we"] else -1
                    child[i]['y'] = parent_pos['son']['h']
                    child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
                        child[i]['y'] - pos['end_h'])) * self.move_long10
                    child[i]['g'] = g + 10
                    child[i]['f'] = child[i]['h'] + child[i]['g']
            elif i == 'ws':
                if child[i]['x'] == 0 and child[i]['y'] == 0:
                    child[i]['x'] = parent_pos['son']['w'] - 1 if parent_pos['son']['w'] - 1 >= 0 else -1
                    child[i]['y'] = parent_pos['son']['h'] + 1 if parent_pos['son']['h'] + 1 < pos["threshold_sn"] else -1
                    child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
                        child[i]['y'] - pos['end_h'])) * self.move_long10
                    child[i]['g'] = g + 14
                    child[i]['f'] = child[i]['h'] + child[i]['g']
            elif i == 's':
                if child[i]['x'] == 0 and child[i]['y'] == 0:
                    child[i]['x'] = parent_pos['son']['w']
                    child[i]['y'] = parent_pos['son']['h'] + 1 if parent_pos['son']['h'] + 1 < pos["threshold_sn"] else -1
                    child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
                        child[i]['y'] - pos['end_h'])) * self.move_long10
                    child[i]['g'] = g + 10
                    child[i]['f'] = child[i]['h'] + child[i]['g']
            elif i == 'es':
                if child[i]['x'] == 0 and child[i]['y'] == 0:
                    child[i]['x'] = parent_pos['son']['w'] + 1 if parent_pos['son']['w'] + 1 < pos["threshold_we"] else -1
                    child[i]['y'] = parent_pos['son']['h'] + 1 if parent_pos['son']['h'] + 1 < pos["threshold_sn"] else -1
                    child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
                        child[i]['y'] - pos['end_h']))*self.move_long10
                    child[i]['g'] = g + 14
                    child[i]['f'] = child[i]['h'] + child[i]['g']
        return child
    
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

判断子节点是否有效，无效则删除（节点是障碍物或者超出地图边缘）

def __getchild_two(self, child):
        mydel = []
        for i in child:
            if child[i]['x'] < 0 or child[i]['y'] < 0:
                mydel.append(i)
            elif self.maps[child[i]['y']][child[i]['x']] == 1:
                mydel.append(i)
            else:
                for n in self.close_list:
                    if n['son']['w'] == child[i]['x'] and n['son']['h'] == child[i]['y']:
                        mydel.append(i)
        for i in mydel:
            child.pop(i)
        return child
1
2
3
4
5
6
7
8
9
10
11
12
13
14

9.PK已存在路径和当前路径，选择最优

 #若当前处理节点的相邻格子已经在Open List中，则检查这条路径是否更优，
    # 即计算经由当前处理节点到达那个方格是否具有更小的 G值。如果没有，不做任何操作。
    # 相反，如果G值更小，则把那个方格的父节点设为当前处理节点 ( 我们选中的方格 )
    # ，然后重新计算那个方格的 F 值和 G 值
    def __child_open_pk(self, child):
        gai_list = []
        num_list = []
        find_list = True
        for i in child:
            open_list = {
                "parent": {'w': self.parent_map['parent']['w'], 'h': self.parent_map['parent']['h']},
                "son": {'w': child[i]['x'], 'h': child[i]['y']},
                "f": child[i]['f'],
                "g": child[i]['g'],
                "h": child[i]['h'],
                "str": self.maps[child[i]['y']][child[i]['x']]}
            if open_list['str'] == 2:
                self.close_list.append(open_list)
                self.end = False
                break
            for n in range(len(self.open_lists)):
                if self.open_lists[n]['son']['w'] == child[i]['x'] and self.open_lists[n]['son']['h'] == child[i]['y']:
                    if child[i]['g'] < self.open_lists[n]['g']:
                        num_list.append(n)
                        gai_list.append(open_list)
                    find_list = False
            if find_list == True:
                gai_list.append(open_list)
            find_list = True

        if gai_list:
            if num_list:
                for n in num_list:
                    self.open_lists.pop(n)
            for i in gai_list:
                self.open_lists.append(i)
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

10.获取最终路径

    def __answer_road(self):
        answer_roads = []
        answer_road = None
        for i in self.close_list:
            if i['str'] == 2:
                answer_roads.append(i)
                answer_road = i
        if answer_road:
            for n in self.close_list:
                for i in self.close_list:
                    if i['son']['w'] == answer_road['parent']['w'] and\
                            i['son']['h'] == answer_road['parent']['h']:
                        answer_roads.append(i)
                        answer_road = i
                        if i['str'] == -1:
                            break
        while True:
            if answer_roads[-2]['str'] == -1:
                answer_roads.pop(-2)
            else:
                break
        for f in answer_roads:
            if f['str'] == 0:
                self.maps[f['son']['h']][f['son']['w']] = 4
        answer_roads.reverse()
        return answer_roads
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

11. 主程序-任务执行

#主函数--任务执行
    def A_navigate(self):
        #初始化坐标
        self.__init_start()
        self.__init_pos(self.maps, -1, 2)
        print("开始: 起点，终点，地图大小 （高，宽）", self.pos)
        #初始化父节点
        self.__init_parent_map()
        #起点加入open list
        self.map_coor = {
            'coor_pos': {'w': self.pos['start_w'], 'h': self.pos['start_h']},
            'f': self.parent_map['f'],
            'g': self.parent_map['g'],
            'h': self.parent_map['h'],
            'walkable': 0
        }
        self.open_lists.append(self.parent_map)
        self.__find_open_fmin()
        child = self.__getchild_one(self.__childer(),self.parent_map,self.pos,self.parent_map['g'])
        child = self.__getchild_two(child)
        for i in child:
            _parent_map = {
                "parent": {'w': self.parent_map['parent']['w'], 'h': self.parent_map['parent']['h']},
                "son": {'w': child[i]['x'], 'h': child[i]['y']},
                "f": child[i]['f'],
                "g": child[i]['g'],
                "h": child[i]['h'],
                "str": self.maps[child[i]['y']][child[i]['x']]}
            self.open_lists.append(_parent_map)
		#循环检测，直到找到最优路径
        while self.end:
            #排序
            self.open_lists.sort(key=lambda i:i['f'], reverse=False)
            self.close_list.append(self.open_lists[0])
            self.parent_map = {
                "parent": {'w': self.open_lists[0]['son']['w'], 'h': self.open_lists[0]['son']['h']},
                "son": {'w': self.open_lists[0]['son']['w'], 'h': self.open_lists[0]['son']['h']},
                "f": self.open_lists[0]['f'],
                "g": self.open_lists[0]['g'],
                "h": self.open_lists[0]['h'],
                "str": self.open_lists[0]['str']
                }
            self.open_lists.pop(0)
            child = self.__getchild_one(self.__childer(), self.parent_map, self.pos, self.parent_map['g'])
            child = self.__getchild_two(child)
            self.__child_open_pk(child)

        for i in self.close_list:
            if i['str'] == 0:
                self.maps[i['son']['h']][i['son']['w']] = 5

        self.list_road = self.__answer_road()

        return self.list_road
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

总结

可以在评论区进行讨论哦！
谢谢大家能点个赞！
希望和大家一起交流！