寻路算法-A star

A*算法是一种启发式的图遍历和路径搜索算法。在1968年，A*算法由美国计算机科学家Peter E. Hart首次提出，用于世界上第一个具备人工智能的机器人SHAKEY的路径规划。

启发式（heuristics）算法：启发式算法无法保证解的最优性omtimal，但是足以达到一个可行的解（It is sufficient for reaching an immediate, short-term goal or approximation）。

加权图（weighted graph）：例如栅格地图、路网图。将栅格地图的一个栅格看作一个节点node。

一、算法

A*算法从起点开始向终点扩展，每次迭代会在邻居中选择一个代价最小的节点。其中的代价使用启发式函数描述。

1. 定义启发式函数描述路径代价

$$f(n)=g(n)+h(n)$$
其中，$g(n)$表示起点到当前节点的代价，是已知的；$h(n)$表示当前节点到终点的代价，$h(n)$是估计值，未知的。

2. 循环扩展的算法步骤

• 步骤1. 将当前节点A加入open list。 open list里面的节点是路径的候选节点，它的第一个节点是起点。
• 步骤2. 将A的可到达的邻居节点加入open list，并且把A设置成这些节点的父亲。 父亲的记录是用于搜索结束后的路径方向追溯。
• 步骤3. 把A从open list中移除，并加入到close list中。 close list中是用于记录已经扩展了邻居的节点，表示以后不需要再次访问的。
• 步骤4. 在open list中，选择启发代价最小的节点最为节点A，启动下一次循环（步骤23）。直到到达终点或者搜索失败。

3. 详细解释

• open list 和 close list是A*算法实现需要的支持数据结构，核心是open list。open list里面的节点是路径的候选节点，它的第一个节点是起点。close list更是一种节点的状态，表示已经扩展了邻居的节点，以后不需要再次访问的。两者通常使用优先级队列（堆）实现，利用了其最大（或最小）元素先出的特性。

  • close list在实现过程中，往往可用标志位的形式描述。

  • 队列具有先进先出的特性，但是优先级队列不再如此。无论节点的入队顺序如何，优先级队列保证先出的一定是最大（或最小）的元素。对于优先级队列，每次入队都是一个插入排序操作。

• 记录父亲是隐藏的链表，在搜索过程中不断增长，直到扩展至终点。然后，从终点开始，不断回溯父亲，直到起点，构成了路径。

• 代价评估。当$h(n)$使用曼哈顿距离时，只能前后作用移动。当$h(n)$使用欧氏距离时，可以斜边移动。

• 示例
  

二、伪代码

/* from 维基百科 */
function reconstruct_path(cameFrom, current)
    total_path := {current}
    while current in cameFrom.Keys:
        current := cameFrom[current]
        total_path.prepend(current)
    return total_path

// A* finds a path from start to goal.
// h is the heuristic function. h(n) estimates the cost to reach goal from node n.
function A_Star(start, goal, h)
    // The set of discovered nodes that may need to be (re-)expanded.
    // Initially, only the start node is known.
    // This is usually implemented as a min-heap or priority queue rather than a hash-set.
    openSet := {start}

    // For node n, cameFrom[n] is the node immediately preceding it on the cheapest path from start
    // to n currently known.
    cameFrom := an empty map //记录节点的来源（父亲节点），用于反向获取路径

    // For node n, gScore[n] is the cost of the cheapest path from start to n currently known.
    gScore := map with default value of Infinity //map是代价地图
    gScore[start] := 0

    // For node n, fScore[n] := gScore[n] + h(n). fScore[n] represents our current best guess as to
    // how cheap a path could be from start to finish if it goes through n.
    fScore := map with default value of Infinity
    fScore[start] := h(start)

    while openSet is not empty
        // This operation can occur in O(Log(N)) time if openSet is a min-heap or a priority queue
        current := the node in openSet having the lowest fScore[] value
        if current = goal
            return reconstruct_path(cameFrom, current)

        openSet.Remove(current)
        for each neighbor of current
            // d(current,neighbor) is the weight of the edge from current to neighbor
            // tentative_gScore is the distance from start to the neighbor through current
            tentative_gScore := gScore[current] + d(current, neighbor)
            if tentative_gScore < gScore[neighbor]
                // This path to neighbor is better than any previous one. Record it!
                cameFrom[neighbor] := current
                gScore[neighbor] := tentative_gScore
                fScore[neighbor] := tentative_gScore + h(neighbor)
                if neighbor not in openSet
                    openSet.add(neighbor)

    // Open set is empty but goal was never reached
    return failure

三、实现

推荐大牛的开源代码：https://github.com/zhm-real/PathPlanning

• 2D
  
• 3D
  

四、参考文献

【1】Hart, P. E.; Nilsson, N.J.; Raphael, B. (1968). “A Formal Basis for the Heuristic Determination of Minimum Cost Paths”. IEEE Transactions on Systems Science and Cybernetics. 4 (2): 100–7. doi:10.1109/TSSC.1968.300136 .