寻路A* g值不断变化

start：路径规划的起始点，也是机器人当前位置或初始位置A goal：路径规划的终点，也是机器人想要到达的位置B g_score：当前点到沿着start点A产生的路径到A点的移动耗费 h_score：不考虑不可通过区域，当前点到goal点B的理论移动耗费 f_score：g_score+h_score，通常也写为F=G+H 开启列表openset：寻路过程中的待检索节点列表 关闭列表closeset：不需要再次检索的节点列表 追溯表comaFrom：存储父子节点关系的列表，用于追溯生成路径。

作者：joey_zhou
链接：https://www.jianshu.com/p/8905d4927d5f
來源：简书
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。

2. 程序主体

• 将起始点start加入开启列表openset
• 重复一下工作
  • 寻找开启列表openset中F值最小的节点，设为当前点current
  • 开启列表openset中移出当前点current
  • 关闭列表openset中加入当前点current
  • 对当前点的每一个相邻点neighbor
    • 如果它不可通过或者已经在关闭列表中，略过。否则：
    • 如果它不在开启列表中，加入开启列表中
    • 如果在开启列表中，G值判定，若此路径G值比之前路径小，则此相邻点的父节点为当前点，同时更新G与F值。反之，则保持原来的节点关系与G、F值。
  • 当目标点goal在开启列表中，则结束程序，此时有路径生成，此时由goal节点开始逐级追溯上一级父节点，直到追溯到开始节点start，此时各节点即为路径。
  • 当开启列表为空，则结束程序，此时没有路径

// a* 伪代码
function A*(start, goal)
    //初始化关闭列表，已判定过的节点，进关闭列表。
    closedSet := {}
    // 初始化开始列表，待判定的节点加入开始列表。
    // 初始openset中仅包括start点。
    openSet := {start}
    // 对每一个节点都只有唯一的一个父节点，用cameFrom集合保存节点的子父关系。  
        //cameFrom（节点）得到父节点。
    cameFrom := the empty map

    // gScore估值集合
    gScore := map with default value of Infinity
    gScore[start] := 0 

    // fScore估值集合
    fScore := map with default value of Infinity
    fScore[start] := heuristic_cost_estimate(start, goal)

    while openSet is not empty
                    //取出F值最小的节点设为当前点
        current := the node in openSet having the lowest fScore[] value
                    //当前点为目标点，跳出循环返回路径
        if current = goal
            return reconstruct_path(cameFrom, current)

        openSet.Remove(current)
        closedSet.Add(current)

        for each neighbor of current

            if neighbor in closedSet
                continue        // 忽略关闭列表中的节点
            // tentative_gScore作为新路径的gScore
            tentative_gScore := gScore[current] + dist_between(current, neighbor)
            if neighbor not in openSet  
                openSet.Add(neighbor)
            else if tentative_gScore >= gScore[neighbor]
                continue        //新gScore>=原gScore，则按照原路径

            // 否则选择gScore较小的新路径，并更新G值与F值。同时更新节点的父子关系。
            cameFrom[neighbor] := current
            gScore[neighbor] := tentative_gScore
            fScore[neighbor] := gScore[neighbor] + heuristic_cost_estimate(neighbor, goal)

    return failure
    //从caomeFrom中从goal点追溯到start点，取得路径节点。
function reconstruct_path(cameFrom, current)
    total_path := [current]
    while current in cameFrom.Keys:
        current := cameFrom[current]
        total_path.append(current)
    return total_path

作者：joey_zhou
链接：https://www.jianshu.com/p/8905d4927d5f
來源：简书
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。