请提供一个A*路径规划算法

A*路径规划算法是一种常用的启发式搜索算法，用于求解最短路径问题。其基本思想是在搜索过程中维护一个估价函数，该函数可以评估从起点到当前节点的最短路径长度，并利用这个估价函数来指导搜索过程，使得搜索能够更快地找到最短路径。

以下是一个简单的A*路径规划算法的伪代码：

function A*(start, goal, heuristic_cost_estimate)
    // 初始化open set和closed set
    open_set = {start}
    closed_set = {}

    // 从起点到当前节点的实际代价
    g_score = {start: 0}

    // 从起点到当前节点的估计代价
    f_score = {start: heuristic_cost_estimate(start, goal)}

    // A*搜索循环
    while open_set is not empty:
        // 从open set中选择f_score最小的节点
        current = node in open_set with the lowest f_score

        // 如果当前节点是目标节点，返回路径
        if current == goal:
            return reconstruct_path(came_from, current)

        // 将当前节点加入closed set
        open_set.remove(current)
        closed_set.add(current)

        // 遍历当前节点的邻居节点
        for neighbor in current.neighbors:
            if neighbor in closed_set:
                // 如果邻居节点已经在closed set中，跳过
                continue

            // 计算从起点到邻居节点的实际代价
            tentative_g_score = g_score[current] + distance(current, neighbor)

            if neighbor not in open_set:
                // 如果邻居节点不在open set中，加入open set
                open_set.add(neighbor)
            else if tentative_g_score >= g_score[neighbor]:
                // 如果邻居节点在open set中，且新代价不优于旧代价，跳过
                continue

            // 更新邻居节点的代价
            came_from[neighbor] = current
            g_score[neighbor] = tentative_g_score
            f_score[neighbor] = g_score[neighbor] + heuristic_cost_estimate(neighbor, goal)

    // 没有找到路径，返回空
    return None

function reconstruct_path(came_from, current)
    // 从终点开始，通过came_from映射逆向重构路径
    path = [current]
    while current in came_from:
        current = came_from[current]
        path.append(current)
    return path.reverse()

其中，`start`表示起点，`goal`表示终点，`heuristic_cost_estimate`是一个估价函数，用于估计从当前节点到目标节点的最短路径长度。在循环过程中，`open_set`存储待考虑的节点，`closed_set`存储已经考虑过的节点。`g_score`表示从起点到当前节点的实际代价，`f_score`表示从起点到当前节点的估计代价。在每次循环中，选择`f_score`最小的节点进行扩展。对于每个邻居节点，计算从起点到该节点的实际代价，并更新`g_score`和`f_score`。如果邻居节点不在`open_set`中，则将其加入；如果已经在`open_set`中，比较新旧代价，更新代价并调整`open_set`。最后，如果找到了路径，通过`came_from`映射逆向重构路径并返回。如果没有找到路径，则返回空。