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]) + "	",
            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