首页 > 代码库 > A*算法的理解与简单实现

A*算法的理解与简单实现

基本定义

一种寻路算法,特点是:启发式的,效率高,基本思路比较简单。

用途

寻路。在指定的地图上,考虑到地图上的移动代价,找到最优的路径。

核心概念

开表,闭表,估值函数。

开表

开表,记录了当前需要处理的地图上的点。

1什么点会加入开表?

1.1    当一个点是起始点时,可以加入;

1.2    当一个点是起始点的邻接点,且不再闭表里时,可以进入开表;

2什么点会离开开表?

2.1开表中的点会按照f(n)进行升序排序,得到最小值的一个点被最先处理;当一个点已经处理后,会离开开表,加入闭表。

闭表

闭表,记录了当前已经处理的地图上的点。

1什么点会加入闭表?

1.1当一个点已经处理后,会加入闭表;

2什么点会离开闭表?

不会有点离开闭表。

估值函数

估值函数,估算了当前点处于最优路径上的代价。

估值函数f(n) = g(n) + h(n),其中g(n)表示由起点到当前点的代价;h(n)表示由当前点到终点的代价。A*算法的最核心部分也就是这个估值函数的设计。

在我的实现中,我用简单的几何距离作为估值函数的实现。

算法描述

1将起点加入开表

2开表里如果为空,算法退出;否则,取出f(n)最小的点作为最优路径上的点;

3针对当前处理的点,计算邻接点,如果邻接点不在闭表,且不在开表,加入开表

4重复2,3步骤直到退出

示例实现

  1 #coding=utf8
  2 """
  3     a* algorithm
  4 """
  5 import math
  6 
  7 AB_MAP = [
  8     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
  9     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 10     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 11     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 12     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 13     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 14     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 15     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 16     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 17     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 18     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 19     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 20     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 21     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 22     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 23     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 24     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 25     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 26     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 27     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 28 ]
 29 POS_UP = 1
 30 POS_LEFT = 2
 31 POS_DOWN = 3
 32 POS_RIGHT = 4
 33 
 34 
 35 def mod_map(m_map, pos_x, pos_y, val):
 36     """
 37         修改地图的某个点位的值
 38     """
 39     m_map[pos_x][pos_y] = val
 40 
 41 
 42 def print_map(m_map):
 43     """
 44         打印地图
 45     """
 46     rows = len(m_map)
 47     for i in range(0, rows - 1):
 48         cols = len(m_map[i])
 49         for j in range(0, cols - 1):
 50             print str(m_map[i][j]) + "\t",
 51             j = j + 1
 52         print
 53         i = i + 1
 54 
 55 
 56 class Node(object):
 57     """
 58         记录一个节点的信息
 59     """
 60     def __init__(self, x, y, h_n=0, g_n=0):
 61         self.pos_x = x
 62         self.pos_y = y
 63         self.parent = None
 64         self.h_n = h_n
 65         self.g_n = g_n
 66         self.f_n = self.h_n + self.g_n
 67 
 68     def update_h_n(self, target):
 69         """
 70             计算h(n)
 71         """
 72         self.h_n = int(math.sqrt((target.pos_x - self.pos_x)**2 + (target.pos_y - self.pos_y)**2))
 73         return self.h_n
 74 
 75     def update_g_n(self, source):
 76         """
 77             计算g(n)
 78         """
 79         self.g_n = int(math.sqrt((source.pos_x - self.pos_x)**2 + (source.pos_y - self.pos_y)**2))
 80         return self.g_n
 81 
 82     def update_f_n(self, source, target):
 83         """
 84             计算f(n)
 85         """
 86         self.f_n = self.update_g_n(source) + self.update_h_n(target)
 87         return self.f_n
 88 
 89     def update_parent(self, par):
 90         """
 91             更新父节点
 92         """
 93         self.parent = par
 94 
 95     def get_adj_node(self, flag, source, target):
 96         """
 97             计算邻近的节点
 98         """
 99         if flag == POS_UP:
100             cur_node = Node(self.pos_x, self.pos_y - 1)
101             cur_node.update_f_n(source, target)
102             cur_node.parent = self
103             return cur_node
104         elif flag == POS_LEFT:
105             cur_node = Node(self.pos_x - 1, self.pos_y)
106             cur_node.update_f_n(source, target)
107             cur_node.parent = self
108             return cur_node
109         elif flag == POS_DOWN:
110             cur_node = Node(self.pos_x, self.pos_y + 1)
111             cur_node.update_f_n(source, target)
112             cur_node.parent = self
113             return cur_node
114         elif flag == POS_RIGHT:
115             cur_node = Node(self.pos_x + 1, self.pos_y)
116             cur_node.update_f_n(source, target)
117             cur_node.parent = self
118             return cur_node
119         else:
120             return None
121 
122 
123 def node_addible(node, open_list, close_list):
124     """
125         判断一个点是否在open和close表
126     """
127     index = str(node.pos_x) + _ + str(node.pos_y)
128     if index not in open_list and index not in close_list:
129         open_list[index] = node
130 
131 
132 def reach_end(node, target):
133     """
134         判断一个点是否到达终点
135     """
136     if node and target and node.pos_x == target.pos_x and node.pos_y == target.pos_y:
137         return True
138     else:
139         return False
140 
141 
142 def handle_reach_end(node, mmap, modval, print_path=False):
143     """
144         当一个点到达终点时的处理方法
145     """
146     if node and mmap:
147         while node:
148             if print_path:
149                 print "x: %s, y: %s" % (node.pos_x, node.pos_y)
150             mod_map(mmap, node.pos_x, node.pos_y, modval)
151             node = node.parent
152 
153 def main(source, target, open_list, close_list, mmap):
154     """
155         主函数
156     """
157     open_list[str(source.pos_x) + _ + str(source.pos_y)] = source
158     while open_list:
159         tmp_dict = sorted(open_list.iteritems(), key=lambda d: d[1].f_n)
160         first_key = tmp_dict[0][0]
161         first_node = open_list[first_key]
162         del open_list[first_key]
163 
164         up_node = first_node.get_adj_node(POS_UP, source, target)
165         if reach_end(up_node, target):
166             handle_reach_end(up_node, mmap, 2)
167             break
168 
169         left_node = first_node.get_adj_node(POS_LEFT, source, target)
170         if reach_end(left_node, target):
171             handle_reach_end(left_node, mmap, 2)
172             break
173 
174         down_node = first_node.get_adj_node(POS_DOWN, source, target)
175         if reach_end(down_node, target):
176             handle_reach_end(down_node, mmap, 2)
177             break
178 
179         right_node = first_node.get_adj_node(POS_RIGHT, source, target)
180         if reach_end(right_node, target):
181             handle_reach_end(right_node, mmap, 2)
182             break
183 
184         if first_key not in close_list:
185             close_list[first_key] = first_node
186 
187         node_addible(up_node, open_list, close_list)
188         node_addible(down_node, open_list, close_list)
189         node_addible(left_node, open_list, close_list)
190         node_addible(right_node, open_list, close_list)
191 
192 
193 if __name__ == __main__:
194     print "******************************* before *******************************"
195     print_map(AB_MAP)
196     OPEN_LIST = {}
197     CLOSE_LIST = {}
198 
199     SOURCE = Node(3, 4)
200     TARGET = Node(13, 9)
201     main(SOURCE, TARGET, OPEN_LIST, CLOSE_LIST, AB_MAP)
202 
203     print "******************************** after ********************************"
204     mod_map(AB_MAP, SOURCE.pos_x, SOURCE.pos_y, 0)
205     mod_map(AB_MAP, TARGET.pos_x, TARGET.pos_y, 0)
206     print_map(AB_MAP)

参考文章

http://www.cnblogs.com/technology/archive/2011/05/26/2058842.html
http://blog.sciencenet.cn/blog-5422-538894.html
https://segmentfault.com/a/1190000004462060
http://blog.csdn.net/v_JULY_v/article/details/6093380
http://blog.csdn.net/zhuangxiaobin/article/details/38755447
http://www.cnblogs.com/tongy0/p/5671545.html

 

A*算法的理解与简单实现