[PathPlanning]AStar.md

[PathPlanning]AStar.md

九月 23, 2019

Ａ*寻路算法

A*搜索算法，俗称Ａ星算法，作为启发式搜错算法中的一种，这是一种在图形平面上，有多个节点路径，求出最低通过成本的算法．长用于游戏中的NPC的移动计算，或线上游戏BOT的移动计算上．该算法想Dijkstar算法一样，可以找到最短路径,也像BFS一样，进行启发式搜索．
AStar

算法思路

  1. 计算路径时主要利用一个公式来筛选最优路径点:
    f(n) = g(n) + h(n)

    其中:

    • fn(n)是节点n的综合优先，我们根据这个值的大小(一般选取最小)来判断该点是否是最优(最短)路径上的点,最终的路径就是有一系列最小点所组成的．
    • g(n)是节点n距离起点的代价.
    • h(n)是节点n距离终点的预计代价,这也就是A*算法的启发函数.

A*算法计算过程中，每次从优先队列中选取f(n)优先级最高(值最小)的节点作为下一个待遍历的点(有点贪心算法的思想).另外Ａ*算法使用两个集合来表示待遍历的点以及已经遍历过的点,通常称之为open_set和close_set.

  1. 完整的Ａ*算法描述如下：

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    * 初始化open_set和close_set;
    * 将起点加入open_set中，并设置优先级为0(优先级最高)
    * 如果open_set不为空,则从open_set中选取优先级最高的节点n:
        * 如果节点n为终点,则：
            * 从终点开始逐步跟踪parent节点,一直到达起点;
            * 返回找到的结果路径,算法结束;
        * 如果节点n不是终点,则:
            * 将节点n从open_set中删除,并加入close_set中;
            * 遍历节点n所有的临近点：
                *如果临近点m在close_set中，则：
                    * 跳过选取下一个临近点
                * 如果临近节点m也不在open_set中，则:
                    * 设置节点m的parent为节点n
                    * 计算节点m的优先级
                    * 将节点m加入open_set中

  2. 启发函数
    启发函数会影响A*算法的行为.

  • 在极端情况下,当启发函数f(n)始终为0,则将由g(n)决定节点的优先级，此时算法退化成Dijkstara算法.
  • 如果h(n)始终小于等于节点n到终点的代价,则Ａ*算法保证一定能够到最短路径．但是当h(n)的值越小，算法遍历越多的节点,也就导致算法越慢.
  • 如果h(n)完全等于n节点到终点的代价,则Ａ*算法将找到最佳路径，并且速度很快,可惜的是,并非所有场景下都能做到这一点,因为在没有到达终点之前，我们很难确确切算出距离终点还有多远.
  • 如果h(n)的值比节点n到终点的代价还要大,则A*算法不能保证找到最短路径.
  • 在另外一个极端情况下,如果h(n)相较于g(n)大很多,则此时只有h(n)产生效果,这也就变成了最佳优先搜索.

由于上面这些信息，我们可以知道通过调节启发函数我们可以控制算法的速度和精确度,因为在一些情况,我们可能未必需要最短路径,而是希望能够尽快找到一个路径即可，这也是Ａ*算法比较灵活的地方．

  1. 对于网格形式的图，有一下这些启发函数可以使用:
  • 如果图形中只允许朝上下左右四个方向移动，则可以使用曼哈顿距离(Manhattan distance)
  • 如果图形中允许朝八个方向移动，则可以使用对角距离.
  • 如果图形中允许朝着任何方向移动，则可以使用欧几里得距离(Euclidean distance)

距离度量

  1. 曼哈顿距离
    如果图形中只允许朝上下左右四个方向移动，则启发函数可以使用曼哈顿距离:

    1
    2
    3
    4
    function heuristic(node) =
        dx = abs(node.x - goal.x)
        dy = abs(node.y - goal.y)
        return D * (dx + dy)

  2. 对角距离
    如果允许朝八个方向移动，使用对角距离,其中D2指的是两个斜着相邻节点之间的移动代价，如果所有节点都是正方形其值为2^1/2 * D:

    1
    2
    3
    4
    function heuristic(node) = 
        dx = abs(node.x - goal.x)
        dy = abs(node.y - goal.y)
        return D * (dx + dy) + (D2 - 2*d) * min(dx, dy)

  3. 欧几里德距离
    如果图形中允许朝任意方向移动，可以使用欧几里德距离:

    1
    2
    3
    4
    function heuristic(node) = 
        dx = abs(node.x - goal.x)
        dy = abs(node.y - goal.y)
        return D * sqrt(dx * dx + dy * dy)

流程

代码分析

这里所使用的代码是由AtsushiSakai所编写的(尊重原创)

坐标点与地图

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
40
41
42
43
44
45
46
47
def main():
    print(__file__ + " start!!")

    # start and goal position
    sx = 10.0  # 起始点的x坐标[m]
    sy = 10.0  # 起始点的y坐标[m]
    gx = 50.0  # 终止点的x坐标[m]
    gy = 50.0  # 终止点的y坐标[m]
    grid_size = 2.0  # 离散点间隔大小[m]
    robot_radius = 1.0  # 机器人半径用于碰撞检测[m]

    # set obstable positions 设置地图
    ox, oy = [], []
    for i in range(-10, 60):
        ox.append(i)
        oy.append(-10.0)
    for i in range(-10, 60):
        ox.append(60.0)
        oy.append(i)
    for i in range(-10, 61):
        ox.append(i)
        oy.append(60.0)
    for i in range(-10, 61):
        ox.append(-10.0)
        oy.append(i)
    #设置障碍物
    for i in range(-10, 40):
        ox.append(20.0)
        oy.append(i)
    for i in range(0, 40):
        ox.append(40.0)
        oy.append(60.0 - i)

    if show_animation:  # pragma: no cover 显示迷宫
        plt.plot(ox, oy, ".k")
        plt.plot(sx, sy, "og")
        plt.plot(gx, gy, "xb")
        plt.grid(True)
        plt.axis("equal")

    #创建 a_star类
    a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
    #启用路径规划
    rx, ry = a_star.planning(sx, sy, gx, gy)
    #显示使用Ａ*规划出来的路径
    if show_animation:  # pragma: no cover
        plt.plot(rx, ry, "-r")

这段代码说明如下:

  1. 首先人为初始化起点[10, 10]和终点[50, 50]
  2. 设置地图大小x(-10…60) y(-10…60)的正方形
  3. 设置地图中的障碍物
  4. 创建astar类调用规划算法

    算法主体

    算法主体是一个类封装的一系列函数:
    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
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    55
    56
    57
    58
    59
    60
    61
    62
    63
    64
    65
    66
    67
    68
    69
    70
    71
    72
    73
    74
    75
    76
    77
    78
    79
    80
    81
    82
    83
    84
    85
    86
    87
    88
    89
    90
    91
    92
    93
    94
    95
    96
    97
    98
    99
    100
    101
    102
    103
    104
    105
    106
    107
    108
    109
    110
    111
    112
    113
    114
    115
    116
    117
    118
    119
    120
    121
    122
    123
    124
    125
    126
    127
    128
    129
    130
    131
    132
    133
    134
    135
    136
    137
    138
    139
    140
    141
    142
    143
    144
    145
    146
    147
    148
    149
    150
    151
    152
    153
    154
    155
    156
    157
    158
    159
    160
    161
    162
    163
    164
    165
    166
    167
    168
    169
    170
    171
    172
    173
    174
    175
    176
    177
    178
    179
    180
    181
    182
    183
    184
    185
    186
    187
    188
    189
    190
    191
    192
    193
    194
    195
    196
    197
    198
    199
    200
    201
    202
    203
    204
    205
    class AStarPlanner:

        def __init__(self, ox, oy, reso, rr):
            """
            Initialize grid map for a star planning
            ox: x position list of Obstacles [m]
            oy: y position list of Obstacles [m]
            reso: grid resolution [m]
            rr: robot radius[m]
            """

            self.reso = reso
            self.rr = rr
            self.calc_obstacle_map(ox, oy)
            self.motion = self.get_motion_model()

        class Node:
            def __init__(self, x, y, cost, pind):
                self.x = x  # index of grid
                self.y = y  # index of grid
                self.cost = cost
                self.pind = pind

            def __str__(self):
                return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)

        def planning(self, sx, sy, gx, gy):
            """
            A star path search
            input:
                sx: start x position [m]
                sy: start y position [m]
                gx: goal x position [m]
                gx: goal x position [m]
            output:
                rx: x position list of the final path
                ry: y position list of the final path
            """
            #将起点节点初始化
            nstart = self.Node(self.calc_xyindex(sx, self.minx),
                               self.calc_xyindex(sy, self.miny), 0.0, -1)
            #终点节点初始化
            ngoal = self.Node(self.calc_xyindex(gx, self.minx),
                              self.calc_xyindex(gy, self.miny), 0.0, -1)

            open_set, closed_set = dict(), dict()
            open_set[self.calc_grid_index(nstart)] = nstart

            while 1:
                if len(open_set) == 0:
                    print("Open set is empty..")
                    break
                #使用lambda函数找出最小的f(n)对应的索引
                c_id = min(
                    open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, 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.minx),
                             self.calc_grid_position(current.y, self.miny), "xc")
                    if len(closed_set.keys()) % 10 == 0:
                        plt.pause(0.001) #等待0.001s

                #如果找到最终点则停止搜索
                if current.x == ngoal.x and current.y == ngoal.y:
                    print("Find goal")
                    ngoal.pind = current.pind
                    ngoal.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计算f(n)
                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
                    #如果节点不再open_set中则将节点放入open_set中
                    if n_id not in open_set:
                        open_set[n_id] = node  # discovered a new node
                    else:
                    #如果节点在open_set中，则判断该节点是否是最优点(代价最小)
                        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(ngoal, closed_set)

            return rx, ry

        def calc_final_path(self, ngoal, closedset):
            # generate final course 从终点反推到起点得出路径
            rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
                self.calc_grid_position(ngoal.y, self.miny)]
            pind = ngoal.pind
            while pind != -1:
                n = closedset[pind]
                rx.append(self.calc_grid_position(n.x, self.minx))
                ry.append(self.calc_grid_position(n.y, self.miny))
                pind = n.pind

            return rx, ry

        @staticmethod
        def calc_heuristic(n1, n2):
            w = 1.0  # weight of heuristic　
            ＃代价函数欧式距离
            d = w * math.sqrt((n1.x - n2.x) ** 2 + (n1.y - n2.y) ** 2)
            return d
        # 索引->点
        def calc_grid_position(self, index, minp):
            """
            calc grid position
            :param index:
            :param minp:
            :return:
            """
            pos = index * self.reso + minp
            return pos
        #　点　->　索引
        def calc_xyindex(self, position, min_pos):
            return round((position - min_pos) / self.reso)

        def calc_grid_index(self, node):
            return (node.y - self.miny) * self.xwidth + (node.x - self.minx)

        def verify_node(self, node):
            px = self.calc_grid_position(node.x, self.minx)
            py = self.calc_grid_position(node.y, self.miny)
            #判断是否超出界限
            if px < self.minx:
                return False
            elif py < self.miny:
                return False
            elif px >= self.maxx:
                return False
            elif py >= self.maxy:
                return False
            # collision check　判断是否是障碍
            if self.obmap[node.x][node.y]:
                return False
            return True

        def calc_obstacle_map(self, ox, oy):
            #计算地图边界
            self.minx = round(min(ox)) 
            self.miny = round(min(oy))
            self.maxx = round(max(ox))
            self.maxy = round(max(oy))
            print("minx:", self.minx)
            print("miny:", self.miny)
            print("maxx:", self.maxx)
            print("maxy:", self.maxy)

            #　计算地图宽度
            self.xwidth = round((self.maxx - self.minx) / self.reso)
            self.ywidth = round((self.maxy - self.miny) / self.reso)
            print("xwidth:", self.xwidth)
            print("ywidth:", self.ywidth)

            # obstacle map generation
            self.obmap = [[False for i in range(self.ywidth)]
                          for i in range(self.xwidth)]
            for ix in range(self.xwidth):
                x = self.calc_grid_position(ix, self.minx)
                for iy in range(self.ywidth):
                    y = self.calc_grid_position(iy, self.miny)
                    for iox, ioy in zip(ox, oy):
                        d = math.sqrt((iox - x) ** 2 + (ioy - y) ** 2)
                        if d <= self.rr:
                            self.obmap[ix][iy] = True
                            break
        """
        (-1,-1)[2*2/1]  (0,-1)[1]   (1, -1)[2*2/1]
        (-1, 0)[１]     (0,0)[0]    (1,0)[1]
        (-1, 1)[2*2/1]  (0,1)[1]    (1,1)[2*2/1]
        """
        @staticmethod
        def get_motion_model():
            # dx, dy, cost
            motion = [[1, 0, 1],
                      [0, 1, 1],
                      [-1, 0, 1],
                      [0, -1, 1],
                      [-1, -1, math.sqrt(2)],
                      [-1, 1, math.sqrt(2)],
                      [1, -1, math.sqrt(2)],
                      [1, 1, math.sqrt(2)]]

            return motion

这段代码说明如下:

  1. __init__: 类构造函数,他需要ox(起始点x坐标), oy(起始点y坐标), reso(离散点的间隔大小), rr(机器人半径)等参数
  2. __str__: 打印类中的具体属性
  3. planning: 规划函数,可以规划出一条从起点到终点的最短路径，其接受四个参数sx(起始点x坐标), sy(起始点y坐标), gx(终点x坐标), gy(终点y坐标)　返回从起点到终点的路径点
  4. calc_final_path: 生成最终轨迹, ngoal(终点索引和代价), colse_set(检索过点的集合)
  5. calc_heuristic: 启发函数采用欧几里德距离,输入两个点计算完成之后返回距离
  6. calc_grid_position: 计算网格位置
  7. calc_xyindex:　计算点x,y坐标索引
  8. calc_grid_index:　计算栅格索引
  9. verify_node:　检验输入的节点是否合法
  10. calc_obstacle_map:　障碍地图生成
  11. get_motion_model:　静态方法，用于制定g(n)的规则
  1. 1. Ａ*寻路算法
    1. 1.1. 算法思路
    2. 1.2. 距离度量
    3. 1.3. 流程
    4. 1.4. 代码分析
      1. 1.4.1. 坐标点与地图
      2. 1.4.2. 算法主体