A*算法及改进

1.A*算法

广度优先搜索算法

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
def breadth_first_seach(graph, start, goal):
    """
    # 广度优先搜索，向四周平等的探索，利用队列存放待探索的点,集合存放已探索的点，
    用字典的键值记录来时的路径，便于溯源，可以找到最短路径
    :param graph: 输入解析出来的图，节点和边组成
    :param start:  起点
    :param goal: 终点
    :return: 返回起点到终点的最短路径
    """
    frontier = Queue()
    frontier.put(start)
    # reached = set()  # 用集合存放到达的方块
    # reached.add(start)
    came_from = dict()  # 用字典的键值记录来时的路径
    came_from[start] = None  # 显然起点来时的路径为空
    while not frontier.empty():
        current = frontier.get()
        if current == goal:
            break
        for next in graph.neighbors(current):
            if next not in came_from:
                frontier.put(next)
                came_from[next] = current
    # 跟随指向从终点回到起点
    current = goal
    path = []
    while current != start:
        path.append(current)
        current = came_from[current]
    path.append(start)
    path.reverse()  # 将顺序正过来，终点到起点，变为起点到终点
    return path

Dijkstra算法

相比广度优先搜索，加入优先队列、cost_so_far来考虑起点到当前节点的代价

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
def dijkstra_seach(graph, start, goal):
    """
    # Dijkstra算法,考虑了成本cost_so_far,使用优先队列保存要探索的点，改变了探索边界扩展的方式
    但还是沿各个方向扩展，可以绕开部分成本多的区域，这是和广度优先搜索四周均匀扩展的区别
    两者都找到了最短路径。
    :param graph: 输入解析出来的图，节点和边组成
    :param start:  起点
    :param goal: 终点
    :return: 返回起点到终点的最短路径
    """
    frontier = PriorityQueue()
    frontier.put(start, 0)
    came_from = dict()  # 用字典的键值记录来时的路径
    came_from[start] = None  # 显然起点来时的路径为空
    cost_so_far = dict()
    cost_so_far[start] = 0

    while not frontier.empty():
        current = frontier.get()
        if current == goal:
            break
        for next in graph.neighbors(current):
            new_cost = cost_so_far[current] + graph.cost(current,next)
            if next not in came_from or new_cost < cost_so_far[next]:
                cost_so_far[next] = new_cost
                priority = new_cost
                frontier.put(next,priority)
                came_from[next] = current
    # 跟随指向从终点回到起点
    current = goal
    path = []
    while current != start:
        path.append(current)
        current = came_from[current]
    path.append(start)
    path.reverse()  # 将顺序正过来，终点到起点，变为起点到终点
    return path,cost_so_far

Greedy_Best_First算法

相比广度优先搜索，加入优先队列、priority，定义了一个函数来考虑和评估当前节点到终点的代价

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
#定义一个曼哈顿距离作为预测中间节点到终点的代价函数
def heuristic(a, b):
    return abs(a.x - b.x) + abs(a.y - b.y)


#贪心搜索算法，算法会沿着一个方向探索，速度块，但不能找到最短路径
def Greedy_Best_First_Search(graph, start, goal):
    """
    #找出的路径不是最短的。因此，当障碍物不多，但路径不好时，该算法运行得更快。
    :param graph: 输入解析出来的图，节点和边组成
    :param start:  起点
    :param goal: 终点
    :return: 返回起点到终点的最短路径
    """
    frontier = PriorityQueue()
    frontier.put(start, 0)
    came_from = dict()  # 用字典的键值记录来时的路径
    came_from[start] = None  # 显然起点来时的路径为空

    while not frontier.empty():
        current = frontier.get()
        if current == goal:
            break
        for next in graph.neighbors(current):
            if next not in came_from:
                priority = heuristic(goal, next)
                frontier.put(next, priority)
                came_from[next] = current
    # 跟随指向从终点回到起点
    current = goal
    path = []
    while current != start:
        path.append(current)
        current = came_from[current]
    path.append(start)
    path.reverse()  # 将顺序正过来，终点到起点，变为起点到终点
    return path

A*算法

两者结合，起点到当前节点、当前节点到终点的代价都考虑，只要使用的启发式算法不高估距离，一定是最短路径。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
# 定义一个曼哈顿距离作为预测中间节点到终点的代价函数
def heuristic(a, b):
    return abs(a.x - b.x) + abs(a.y - b.y)
def a_star_search(graph, start, goal):
    """
    改进的A_star算法：
    结合Greedy_Best_First_Search和Dijkstra算法，既考虑起点到中间节点的距离，也考虑中间节点到终点的距离，是最好的方法
    :param graph:
    :param start:
    :param goal:
    :return:
    """
    frontier = PriorityQueue()  # 用优先队列存放一轮探索过的边界方块，通过方块代价自动排序，每次取出代价最低的方块。
    frontier.put(start, 0)  # 在队列里存放起点
    came_from = dict() # 从当前方块到之前方块的映射，代表路径的来向
    cost_so_far = dict()  # 从起点到当前方块的当前代价
    came_from[start] = None
    cost_so_far[start] = 0
    while not frontier.empty():
        current = frontier.get()

        if current == goal:
            break
        for next in graph.neighbors(current):
            new_cost = cost_so_far[current] + graph.cost(current, next)
            if next not in cost_so_far or new_cost < cost_so_far[next]:
                cost_so_far[next] = new_cost
                priority = new_cost + heuristic(goal, next)
                frontier.put(next, priority)
                came_from[next] = current
    # 跟随指向从终点回到起点
    current = goal
    path = []
    while current != start:
        path.append(current)
        current = came_from[current]
    path.append(start)
    path.reverse()  # 将顺序正过来，终点到起点，变为起点到终点
    return came_from, cost_so_far

总结

算法 搜索方向 考虑因素 是否最短路径 应用场景
广度优先搜索 四周、平等 每个点代价相等 是 查找到所有位置的路径，移动成本相同
dijkstra 多个方向，包含无前途的方向 只考虑中间节点到起点的代价 是 查找到所有位置的路径，移动成本不同
Greedy_Best_First 只搜索有希望的方向 只考虑中间节点到终点的代价 不一定 查找到一个位置的路径，可以接受不是最优
A* 最短路径方向 起点到中点、中点到终点均考虑 是 查找到一个位置的最短路径

2.相关改进

1.启发式函数

$$
f(n) = g(n) + (w+p)*h(n)
$$
  • g(n)是实际代价
  • h(n)是预估代价函数,可使用欧拉、曼哈顿距离、切比雪夫距离等
  • w是权重系数，用于平衡搜索速度和最优路径的权重，w越大，搜索速度越快，但最优路径可能变得不准确；p越大，搜索速度越慢，但最优路径更加准确。
  • p是一个微小的偏移量，当两个节点代价相同时优先选择离终点更近的节点

2.JPS和JPS+

JPS（jump point search）算法实际上是对A* 寻路算法的一个改进,A* 算法在扩展节点时会把节点所有邻居都考虑进去，这样openlist中点的数量会很多，搜索效率较慢。JPS算法则是希望直线方向上中途的点不用放入openlist，只放入每段直线子路径的起点和终点，提高搜索效率。根据强迫邻居和跳点的概念，JPS算法可以有效地跳过一些不必要的节点，如果提前存储好跳点数据则为JPS+算法。

寻路算法可视化网站:https://qiao.github.io/PathFinding.js/visual/

JPS算法论文:http://grastien.net/ban/articles/hg-aaai11.pdf

强迫邻居

节点 x 的8个邻居中有障碍，且 x 的父节点 p 经过x 到达 n 的距离代价比不经过 x 到达的 n 的任意路径的距离代价小，则称 n 是 x 的强迫邻居。即由于障碍物的存在，导致从 p 到 n 的最短路径必然经过 x。

跳点

如果当前点 x 满足以下三个条件之一，则称 x 为跳点：

  • a.节点 x 是起点/终点
  • b.节点 x 至少有一个强迫邻居
  • c.如果父节点在斜方向（意味着这是斜向搜索），节点x的水平或垂直方向上有满足条件a，b的点

舍弃或扩展邻域

舍弃邻域：根据起点与终点的夹角可用舍弃某三个方向，但在特殊地图中不可用，最好局部使用
扩展邻域：由八方向扩展更多的邻域，一次计算多个格子，障碍物范围过小时有穿墙风险

双向A*

使用两个A*搜索，称为Astar1和Astar2，互相使用对方openlist中最小代价点作为终点，直至相遇或为空退出。可以不同步，比如优先处理节点数量少的一方。

openlist数据结构优化

无序数组或者链表插入不耗时，但查找耗时；有序数组或者链表插入耗时，查找不耗时。可以考虑有序跳表、二叉堆、hash表等数据结构，只关注最值,不关注顺序。

弧形优化

贝塞尔曲线

1
2
3
4
5
6
7
8
9
10
11
12
#  贝塞尔曲线部分的三个函数
def comb(n, k):
    return factorial(n) // (factorial(k) * factorial(n-k))

def get_bezier_curve(points):
    n = len(points) - 1
    return lambda t: sum(comb(n, i)*t**i * (1-t)**(n-i)*points[i] for i in range(n+1))

def evaluate_bezier(points, total):
    bezier = get_bezier_curve(points)
    new_points = np.array([bezier(t) for t in np.linspace(0, 1, total)])
    return new_points[:, 0], new_points[:, 1]