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步骤直到退出

示例实现

 #coding=utf8
 """
     a* algorithm
 """
 import math

 AB_MAP = [
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
     [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,],
 ]
 POS_UP = 1
 POS_LEFT = 2
 POS_DOWN = 3
 POS_RIGHT = 4

 def mod_map(m_map, pos_x, pos_y, val):
     """
         修改地图的某个点位的值
     """
     m_map[pos_x][pos_y] = val

 def print_map(m_map):
     """
         打印地图
     """
     rows = len(m_map)
     for i in range(0, rows - 1):
         cols = len(m_map[i])
         for j in range(0, cols - 1):
             print str(m_map[i][j]) + "\t",
             j = j + 1
         print
         i = i + 1

 class Node(object):
     """
         记录一个节点的信息
     """
     def __init__(self, x, y, h_n=0, g_n=0):
         self.pos_x = x
         self.pos_y = y
         self.parent = None
         self.h_n = h_n
         self.g_n = g_n
         self.f_n = self.h_n + self.g_n

     def update_h_n(self, target):
         """
             计算h(n)
         """
         self.h_n = int(math.sqrt((target.pos_x - self.pos_x)**2 + (target.pos_y - self.pos_y)**2))
         return self.h_n

     def update_g_n(self, source):
         """
             计算g(n)
         """
         self.g_n = int(math.sqrt((source.pos_x - self.pos_x)**2 + (source.pos_y - self.pos_y)**2))
         return self.g_n

     def update_f_n(self, source, target):
         """
             计算f(n)
         """
         self.f_n = self.update_g_n(source) + self.update_h_n(target)
         return self.f_n

     def update_parent(self, par):
         """
             更新父节点
         """
         self.parent = par

     def get_adj_node(self, flag, source, target):
         """
             计算邻近的节点
         """
         if flag == POS_UP:
             cur_node = Node(self.pos_x, self.pos_y - 1)
             cur_node.update_f_n(source, target)
             cur_node.parent = self
             return cur_node
         elif flag == POS_LEFT:
             cur_node = Node(self.pos_x - 1, self.pos_y)
             cur_node.update_f_n(source, target)
             cur_node.parent = self
             return cur_node
         elif flag == POS_DOWN:
             cur_node = Node(self.pos_x, self.pos_y + 1)
             cur_node.update_f_n(source, target)
             cur_node.parent = self
             return cur_node
         elif flag == POS_RIGHT:
             cur_node = Node(self.pos_x + 1, self.pos_y)
             cur_node.update_f_n(source, target)
             cur_node.parent = self
             return cur_node
         else:
             return None

 def node_addible(node, open_list, close_list):
     """
         判断一个点是否在open和close表
     """
     index = str(node.pos_x) + '_' + str(node.pos_y)
     if index not in open_list and index not in close_list:
         open_list[index] = node

 def reach_end(node, target):
     """
         判断一个点是否到达终点
     """
     if node and target and node.pos_x == target.pos_x and node.pos_y == target.pos_y:
         return True
     else:
         return False

 def handle_reach_end(node, mmap, modval, print_path=False):
     """
         当一个点到达终点时的处理方法
     """
     if node and mmap:
         while node:
             if print_path:
                 print "x: %s, y: %s" % (node.pos_x, node.pos_y)
             mod_map(mmap, node.pos_x, node.pos_y, modval)
             node = node.parent

 def main(source, target, open_list, close_list, mmap):
     """
         主函数
     """
     open_list[str(source.pos_x) + '_' + str(source.pos_y)] = source
     while open_list:
         tmp_dict = sorted(open_list.iteritems(), key=lambda d: d[1].f_n)
         first_key = tmp_dict[0][0]
         first_node = open_list[first_key]
         del open_list[first_key]

         up_node = first_node.get_adj_node(POS_UP, source, target)
         if reach_end(up_node, target):
             handle_reach_end(up_node, mmap, 2)
             break

         left_node = first_node.get_adj_node(POS_LEFT, source, target)
         if reach_end(left_node, target):
             handle_reach_end(left_node, mmap, 2)
             break

         down_node = first_node.get_adj_node(POS_DOWN, source, target)
         if reach_end(down_node, target):
             handle_reach_end(down_node, mmap, 2)
             break

         right_node = first_node.get_adj_node(POS_RIGHT, source, target)
         if reach_end(right_node, target):
             handle_reach_end(right_node, mmap, 2)
             break

         if first_key not in close_list:
             close_list[first_key] = first_node

         node_addible(up_node, open_list, close_list)
         node_addible(down_node, open_list, close_list)
         node_addible(left_node, open_list, close_list)
         node_addible(right_node, open_list, close_list)

 if __name__ == '__main__':
     print "******************************* before *******************************"
     print_map(AB_MAP)
     OPEN_LIST = {}
     CLOSE_LIST = {}

     SOURCE = Node(3, 4)
     TARGET = Node(13, 9)
     main(SOURCE, TARGET, OPEN_LIST, CLOSE_LIST, AB_MAP)

     print "******************************** after ********************************"
     mod_map(AB_MAP, SOURCE.pos_x, SOURCE.pos_y, 0)
     mod_map(AB_MAP, TARGET.pos_x, TARGET.pos_y, 0)
     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