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()