全局路径规划——A*算法_权重a*算法全覆盖

与Dijkstra算法类似，A*算法结合了贪心算法（深度优先）和Dijkstra算法（广度优先），是一种启发式算法。

路径评价标准是：f(n) = g(n)+h(n)

f(n)是从初始点经过状态N到目标点的代价估计

g(n)是初始点到状态n的实际代价

h(n)是从状态n到目标点的代价

也是使用两个状态表，一个寄存待考察的状态点，一个寄存已经考察的状态点，类似Dijkstra中的U和S列表

算法思想：

1.将地图栅格化，把每一个正方形格子的中央称为节点;

2.确定栅格属性，即每一个格子有两种状态:可走和不可走

3.定义两个列表集合:openList和closeList。openList表由待考察的节点组成, closeList表由已经考察过的节点组成。类似Dijikstra算法的U集合和S集合。

4.确定起始节点和目标节点。

5.初始时，定义A为父节点，节点A离自身距离为0，路径完全﹔确定，移入closeList中;

6.父节点A周围共有8个节点，定义为子节点。将子节点放入i  openList中，成为待考察对象。

7.若某个节点既未在openList，也没在closeList中，则表明还未搜索到该节点。若某个节点在openList中，则对比取f小的。

8.路径优劣判断依据是移动代价，单步移动代价采取Manhattan计算方式，即把横向和纵向移动一个节点的代价定义为10。斜向移动代价参考等腰三角形计算斜边的方式∶，距离为14。

        while 1:
            if len(open_set) == 0:
                print("Open set is empty..")
                break
 
            c_id = min(
                open_set,
                key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node,
                                                                     open_set[
                                                                         o]))
            current = open_set[c_id]
 
            # show graph
            if show_animation:  # pragma: no cover
                plt.plot(self.calc_grid_position(current.x, self.min_x),
                         self.calc_grid_position(current.y, self.min_y), "xc")
                # for stopping simulation with the esc key.
                plt.gcf().canvas.mpl_connect('key_release_event',
                                             lambda event: [exit(
                                                 0) if event.key == 'escape' else None])
                if len(closed_set.keys()) % 10 == 0:
                    plt.pause(0.001)
 
            if current.x == goal_node.x and current.y == goal_node.y:
                print("Find goal")
                goal_node.parent_index = current.parent_index
                goal_node.cost = current.cost
                break
 
            # Remove the item from the open set
            del open_set[c_id]
 
            # Add it to the closed set
            closed_set[c_id] = current
 
            # expand_grid search grid based on motion model
            for i, _ in enumerate(self.motion):
                node = self.Node(current.x + self.motion[i][0],
                                 current.y + self.motion[i][1],
                                 current.cost + self.motion[i][2], c_id)
                n_id = self.calc_grid_index(node)
 
                # If the node is not safe, do nothing
                if not self.verify_node(node):
                    continue
 
                if n_id in closed_set:
                    continue
 
                if n_id not in open_set:
                    open_set[n_id] = node  # discovered a new node
                else:
                    if open_set[n_id].cost > node.cost:
                        # This path is the best until now. record it
                        open_set[n_id] = node
 
        rx, ry = self.calc_final_path(goal_node, closed_set)
 
        return rx, ry

代码讲解：

A*算法的代码实现与Dijkstra类似，不同点在于代价函数不一样，所以在计算U集合中的代价最小点的时候要考虑h和g。

改进：双向A*

def searching_control(start, end, bound, obstacle):
    """manage the searching process, start searching from two side"""
    # initial origin node and end node
    origin = Node(coordinate=start, H=hcost(start, end))
    goal = Node(coordinate=end, H=hcost(end, start))
    # list for searching from origin to goal
    origin_open: list = [origin]
    origin_close: list = []
    # list for searching from goal to origin
    goal_open = [goal]
    goal_close: list = []
    # initial target
    target_goal = end
    # flag = 0 (not blocked) 1 (start point blocked) 2 (end point blocked)
    flag = 0  # init flag
    path = None
    while True:
        # searching from start to end
        origin_open, origin_close = \
            find_path(origin_open, origin_close, target_goal, bound)
        if not origin_open:  # no path condition
            flag = 1  # origin node is blocked
            draw_control(origin_close, goal_close, flag, start, end, bound,
                         obstacle)
            break
        # update target for searching from end to start
        target_origin = min(origin_open, key=lambda x: x.F).coordinate
 
        # searching from end to start
        goal_open, goal_close = \
            find_path(goal_open, goal_close, target_origin, bound)
        if not goal_open:  # no path condition
            flag = 2  # goal is blocked
            draw_control(origin_close, goal_close, flag, start, end, bound,
                         obstacle)
            break
        # update target for searching from start to end
        target_goal = min(goal_open, key=lambda x: x.F).coordinate
 
        # continue searching, draw the process
        stop_sign, path = draw_control(origin_close, goal_close, flag, start,
                                       end, bound, obstacle)
        if stop_sign:
            break
    return path

代码讲解：

原理就是分别在start和end开始做A*寻找，不一样的是，在下一次迭代时，从start来看她的goal就不是最原始的了，而是goal中U集（open集）中代价最小的那个点作为下一次start搜寻的目标点。

同理，从goal方向寻找的就是start中U集（open集）中代价最小的那个点作为下一次end搜寻的目标点。

其实还有很多关于A*算法的思考

加权A*：

f(p) = g(p) + w * h(p)

另一种思考方式是Dijsktra的算法仅使用，而贪婪的最佳优先搜索仅使用。权重是一种在这两种算法之间平滑插值的方法，其中权重 0 表示 Dijkstra 算法，权重 ∞ 表示贪婪最佳优先搜索。权重 1.0 介于两个极端之间，给出 A*。加权 A* 介于 A* 和贪婪最佳优先搜索之间

动态加权A*：

f(p) = g(p) + w(p) * h(p)

可能有很多方法可以改变重量，我的直觉使我相信，最好在路径的起点附近使用较高的权重，而在路径的末尾使用较低的权重。这是因为我认为补偿启发式的权重太低，而且它在路径的开头通常太低，而在路径的末尾附近相当准确。然而，研究表明，我的直觉是错误的。最好在路径尽头附近施加重量。

f(p) = g(p) < (2*w - 1) * h(p)
     ? g(p) / (2*w - 1) + h(p)
     : (g(p) + h(p)) / w

f(p) = g(p) < h(p)
     ? g(p) + h(p)
     : (g(p) + (2*w - 1) * h(p)) / w}

动态A*：

A* 有一些变体允许在计算初始路径后更改世界。D* 适用于您没有完整信息的情况。如果您没有所有信息，A* 可能会犯错误;D*的贡献在于它可以在不花费太多时间的情况下纠正这些错误。LPA* 适用于成本变化时使用。使用 A*，路径可能会因地图更改而失效;LPA* 可以重用以前的 A* 计算来生成新路径。

但是，D* 和 LPA* 都需要大量空间 — 基本上您运行 A* 并保留其内部信息（集合、路径树、值），然后当地图更改时，D* 或 LPA* 会告诉您是否需要调整路径以考虑地图更改

跳转点搜索A*

许多加速 A* 的技术实际上都是为了减少节点的数量。在成本统一的方形网格中，一次查看一个单独的网格空间是一种浪费。一种方法是构建关键点（例如角落）的图形并将其用于寻路。但是，您不想预先计算航点图，请查看跳跃点搜索，这是 A* 的变体，可以在方形网格上向前跳过。当考虑当前节点的子节点是否可能包含在 OPEN 集中时，跳转点搜索会跳到从当前节点可见的远处节点。每个步骤的成本更高，但步骤更少，从而减少了 OPEN 集中的节点数量。请参阅此博客文章有关详细信息，此博客文章一个很好的视觉解释，以及Reddit上的讨论的利弊。

另请参阅矩形对称性缩减，这将分析地图并嵌入到图形本身中。这两种技术都是为方形网格开发的。这是为六边形网格扩展的算法.

波束搜索BeanA*

波束搜索简单说就是给open_set集一个大小限制  防止一些不必要的扩展

迭代加深IDA*

在 IDA* 中，“深度”是值的截止值。当值太大时，甚至不会考虑该节点（即，它不会被添加到 OPEN 集中）。第一次通过你处理的节点很少。每次后续传递，您都会增加访问的节点数。如果你发现路径有所改善，那么你继续增加截止值;否则，您可以停止。

Theta*

A*算法只在8邻域内搜索，而Theta*算法可以有更多的方向选择。

为了改进路径平滑，我们需要在寻路过程中考虑任意角度的线段长度。θ* 算法这样做。在构建父指针时，如果有指向该节点的视线，它将直接指向祖先，并跳过两者之间的节点。它产生的路径比路径平滑更短。但是，Theta* 不保证最短路径。