A*路径规划探究

1.文章说明

A是常用的路径规划算法，在游戏和机器人中经常会用到，这篇文章目的在于记录A的一些原理和要点

2.A*的一些重要概念

• 1）G:该点到起点所需代价，与历史路径有关
• 2）H:曼哈顿距离，该点到终点的距离，与历史路径无关。
• 3）open list: 查找最优路径，需要遍历的列表
• 4）close list:已遍历的的点，或者是障碍物的点

3.A*的步骤

• 1）把起点加入open list
• 2） 重复以下步骤
• a:遍历open list，查找F值最小的节点，把他作为当前要处理的节点
• b:把这个节点移到close list
• c:对当前方格的8个相邻方格的每一个方格做如下操作：
• i)如果他是不可达的或者他在close list中，忽略他，否则
• ii) 如果它不在open list中，把它加入open list，并且把当前方格设置为他的父亲，记录该方格的F,G,H值。
• iii）如果他已经在open list中，检查这条路径是否是更好，用G值做参考，更小的G值，表示这是更好的路径，如果是这样，把他的父亲设置为当前方格，并重新计算他的G值和F 值。
• iiii) 如果终点已经在open list中，此时说明路径已经找到了，退出。

demo(注，代码引用自wang.zhigang@hotmail.com）

#!/usr/bin/python​
# vim:set fileencoding=utf-8
import sys

_2dmap     = []
start      = None
end        = None
open_list  = {}
close_list = {}
map_border = ()

class Node:
    def __init__(this, father, x, y):
        if x < 0 or x >= map_border[0] or y < 0 or y >= map_border[1]:
            raise Exception("node position can't beyond the border!")

        this.father = father
        this.x = x
        this.y = y
        if father != None:
            G2father = calc_G(father, this)
            if not G2father:
                raise Exception("father is not valid!")
            this.G = G2father + father.G
            this.H = calc_H(this, end)
            this.F = this.G + this.H
        else:
            this.G = 0
            this.H = 0
            this.F = 0

    def reset_father(this, father, new_G):
        if father != None:
            this.G = new_G
            this.F = this.G + this.H

        this.father = father

def calc_G(node1, node2):
    x1 = abs(node1.x-node2.x) 
    y1 = abs(node1.y-node2.y) 
    if (x1== 1 and y1 == 0):
        return 10 # same row
    if (x1== 0 and y1 == 1):
        return 10 # same col
    if (x1== 1 and y1 == 1):
        return 14 # cross
    else:
        return 0

def calc_H(cur, end):
    return abs(end.x-cur.x) + abs(end.y-cur.y)

# NOTE 这个地方可能成为性能瓶颈
def min_F_node():
    if len(open_list) == 0:
        raise Exception("not exist path!")

    _min = 9999999999999999
    _k = (start.x, start.y)
    for k,v in open_list.items():
        if _min > v.F:
            _min = v.F
            _k = k
    return open_list[_k]

# 把相邻节点加入open list, 如果发现终点说明找到了路径
def addAdjacentIntoOpen(node):
    # 将该节点从开放列表移到关闭列表当中。
    open_list.pop((node.x, node.y))
    close_list[(node.x, node.y)] = node

    _adjacent = []
    # 相邻节点要注意边界的情况
    try:
        _adjacent.append(Node(node , node.x - 1 , node.y - 1))
    except Exception:
        pass
    try:
        _adjacent.append(Node(node , node.x     , node.y - 1))
    except Exception:
        pass
    try:
        _adjacent.append(Node(node , node.x + 1 , node.y - 1))
    except Exception:
        pass
    try:
        _adjacent.append(Node(node , node.x + 1 , node.y))
    except Exception:
        pass
    try:
        _adjacent.append(Node(node , node.x + 1 , node.y + 1))
    except Exception:
        pass
    try:
        _adjacent.append(Node(node , node.x     , node.y + 1))
    except Exception:
        pass
    try:
        _adjacent.append(Node(node , node.x - 1 , node.y + 1))
    except Exception:
        pass
    try:
        _adjacent.append(Node(node , node.x - 1 , node.y))
    except Exception:
        pass

    for a in _adjacent:
        if (a.x,a.y) == (end.x, end.y):
            new_G = calc_G(a, node) + node.G
            end.reset_father(node, new_G)
            print ("find path finish!")
            return True
        if (a.x,a.y) in close_list:
            continue

        if (a.x,a.y) not in open_list:
            open_list[(a.x,a.y)] = a
        else:
            exist_node = open_list[(a.x,a.y)]
            new_G = calc_G(a, node) + node.G
            if new_G < exist_node.G:
                exist_node.reset_father(node, new_G)

    return False

def find_the_path(start, end):
    open_list[(start.x, start.y)] = start

    the_node = start
    try:
        while not addAdjacentIntoOpen(the_node):
            the_node = min_F_node()
    except Exception:
        # path not exist
        return False

    return True

#=======================================================================
def print_map():
    print ('    Y')
    for i in range(len(_2dmap)):
        print (i),
    print
    print ('  X')
    row = 0
    for l in _2dmap:
        print ('%3d'%row,' ')
        row = row+1
        for i in l:
            print (i, ' ')
        print

def mark_path(node):
    if node.father == None:
        return

    _2dmap[node.x][node.y] = '#'
    mark_path(node.father)

def preset_map():
    global start,end,map_border
    _2dmap.append('S X . . . . . . . . . . . . . X . . . .'.split())
    _2dmap.append('. X . . . . . . . . . . . . . X . . . .'.split())
    _2dmap.append('. X . . . . . . . . . . . . . X . . . .'.split())
    _2dmap.append('. . . . . . . . . . . . . . . X . . . .'.split())
    _2dmap.append('. . . . . . . . . . . . . . . X . . . .'.split())
    _2dmap.append('. . . . . . . . . . . . . . . . . . . .'.split())
    _2dmap.append('. . . . . . . . . . . . . . . X X X X .'.split())
    _2dmap.append('. . . . . . . . . . . . . . . X . . . .'.split())
    _2dmap.append('. . . . . . . . . . . . . . . X . X X X'.split())
    _2dmap.append('. . . . . . . . . . . . . . . X . X . .'.split())
    _2dmap.append('. . . . . . . . . . . . . . . X . . . .'.split())
    _2dmap.append('. . . . . . . . . . . . . . . X . X . .'.split())
    _2dmap.append('. . . . . . . . . . . . . . . X . X . .'.split())
    _2dmap.append('. . . . . . . . . . . . . . . X . X . .'.split())
    _2dmap.append('. . . . . . . . . . . . . . . X . X . .'.split())
    _2dmap.append('. . . . . . . . . . . . . . . X . X . E'.split())
    map_border = (len(_2dmap),len(_2dmap[0]))

    row_index = 0
    for row in _2dmap:
        col_index = 0
        for n in row:
            if n == 'X':
                block_node = Node(None, row_index, col_index)
                close_list[(block_node.x, block_node.y)] = block_node
            elif n == 'S':
                start = Node(None, row_index, col_index)
            elif n == 'E':
                end = Node(None, row_index, col_index)
            col_index = col_index + 1
        row_index = row_index + 1

if __name__=='__main__':
    if len(sys.argv) < 3:
        preset_map()
    else:
        x = int(sys.argv[1])
        y = int(sys.argv[2])
        map_border = (x,y)

        _start = raw_input('pls input start point:')
        _end   = raw_input('pls input end point:')
        _start = _start.split(',')
        _end   = _end.split(',')
        _start = (int(_start[0]), int(_start[1]))
        _end   = (int(_end[0]), int(_end[1]))
        start = Node(None, _start[0], _start[1])
        end = Node(None, _end[0], _end[1])
        # gen map
        _2dmap = [['.' for i in xrange(y)] for i in xrange(x) ]
        # put start and end
        _2dmap[_start[0]][_start[1]] = 'S'
        _2dmap[_end[0]][_end[1]]     = 'E'
        # input blocks
        while True:
            _block = raw_input('input block:')
            if not _block:
                break

            _block = _block.split(',')
            _block = (int(_block[0]), int(_block[1]))
            _2dmap[_block[0]][_block[1]] = 'X'
            block_node = Node(None, _block[0], _block[1])
            close_list[(block_node.x, block_node.y)] = block_node

    print ("orignal map:")
    print_map()

    if find_the_path(start, end):
        mark_path(end.father)
        print ("found road as follow:")
        print_map()
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