路径规划算法之A*算法（附代码及函数功能和参数详细注释）_a*算法源码

路径规划算法之A*算法（附代码及函数功能和参数详细注释）

效果如下

A*（A star）算法是一种在图或网络中寻找从起始节点到目标节点的最短路径的启发式搜索算法。A* 算法结合了 Dijkstra 算法的最短路径搜索和贪心算法的启发式搜索，因此在许多情况下比其他算法更高效地找到最优路径。它常用于路径规划、游戏中的路径查找以及人工智能领域的图搜索问题。

A* 算法的主要思想是维护两个集合：一个是已探索的节点集合，另一个是待探索的节点集合。算法在每一步选择待探索集合中的节点，计算该节点的代价（包括实际代价和启发式代价），然后选取代价最小的节点进行探索。这种选择过程通过使用启发式函数（估计从当前节点到目标节点的代价）来指导，从而使搜索更加方向性和高效。

A* 算法的伪代码如下：

初始化开始节点，将其加入待探索集合。
当待探索集合不为空时：
a. 从待探索集合中选择代价最小的节点作为当前节点。
b. 如果当前节点是目标节点，则搜索完成，回溯生成最优路径。
c. 否则，将当前节点从待探索集合移至已探索集合，并考虑扩展其邻居节点。
d. 计算邻居节点的代价，并将其添加到待探索集合（如果未加入）或更新已有节点的代价（如果更小）。
如果待探索集合为空且未找到目标节点，则路径不存在。
A* 算法的关键在于如何计算节点的代价。代价通常由两部分组成：从起始节点到当前节点的实际代价（即路径长度）和从当前节点到目标节点的启发式代价。这两个代价的权重可以根据问题调整，从而平衡实际代价和启发式代价的影响。

总之，A* 算法通过不断选择代价最小的节点进行搜索，同时利用启发式代价指导搜索方向，能够高效地找到图或网络中的最短路径。

函数功能详细注释

以下是代码中各个重要函数的详细解释：

1. __init__(self, ox, oy, resolution, rr):

   • 参数：
     • ox：障碍物的 x 坐标列表
     • oy：障碍物的 y 坐标列表
     • resolution：网格的分辨率
     • rr：机器人的半径
   • 功能：初始化 A* 路径规划器的各项参数，并调用 calc_obstacle_map 函数计算障碍物地图。
   • 返回值：无

2. planning(self, sx, sy, gx, gy):

   • 参数：
     • sx：起始点的 x 坐标
     • sy：起始点的 y 坐标
     • gx：目标点的 x 坐标
     • gy：目标点的 y 坐标
   • 功能：执行 A* 路径搜索算法，找到从起始点到目标点的最优路径。
   • 返回值：最优路径的 x 坐标列表 rx 和 y 坐标列表 ry。

3. calc_final_path(self, goal_node, closed_set):

   • 参数：
     • goal_node：目标节点，表示找到的最终目标节点
     • closed_set：已探索节点的集合
   • 功能：从目标节点出发，通过 closed_set 回溯计算出最终路径。
   • 返回值：最优路径的 x 坐标列表 rx 和 y 坐标列表 ry。

4. calc_heuristic(n1, n2):

   • 参数：
     • n1：第一个节点
     • n2：第二个节点
   • 功能：计算两个节点之间的启发式代价，用于引导搜索方向。
   • 返回值：两个节点之间的代价值。

5. calc_grid_position(self, index, min_position):

   • 参数：
     • index：网格索引
     • min_position：最小位置值
   • 功能：将网格索引转换为实际坐标位置。
   • 返回值：实际坐标位置。

6. calc_xy_index(self, position, min_pos):

   • 参数：
     • position：位置坐标
     • min_pos：最小位置值
   • 功能：将实际坐标位置转换为网格索引。
   • 返回值：网格索引。

7. calc_grid_index(self, node):

   • 参数：
     • node：节点对象
   • 功能：将节点对象转换为在网格中的索引。
   • 返回值：网格索引。

8. verify_node(self, node):

   • 参数：
     • node：待验证的节点对象
   • 功能：验证节点是否合法，即是否在地图内且没有碰撞障碍物。
   • 返回值：布尔值，表示节点是否合法。

9. calc_obstacle_map(self, ox, oy):

   • 参数：
     • ox：障碍物的 x 坐标列表
     • oy：障碍物的 y 坐标列表
   • 功能：根据障碍物位置信息生成障碍物地图。
   • 返回值：无

10. get_motion_model():

    • 参数：无
    • 功能：返回机器人的运动模型，包括不同运动方向的步长和代价。
    • 返回值：运动模型列表。

11. main():

    • 参数：无
    • 功能：主函数，定义起点、终点、障碍物信息，执行路径规划，并绘制路径和地图。
    • 返回值：无

这些函数相互协作，实现了A*路径规划算法中的关键步骤，包括初始化、节点扩展、代价计算、路径回溯等。在主函数中，初始化地图和路径规划器后，通过调用 planning 函数找到最优路径，并将路径和地图用 matplotlib 进行可视化展示。

具体代码：

import math

import matplotlib.pyplot as plt

show_animation = True


class AStarPlanner:

    def __init__(self, ox, oy, resolution, rr):
        """
        Initialize grid map for a star planning

        ox: x position list of Obstacles [m]
        oy: y position list of Obstacles [m]
        resolution: grid resolution [m]
        rr: robot radius[m]
        """

        self.resolution = resolution
        self.rr = rr
        self.min_x, self.min_y = 0, 0
        self.max_x, self.max_y = 0, 0
        self.obstacle_map = None
        self.x_width, self.y_width = 0, 0
        self.motion = self.get_motion_model()
        self.calc_obstacle_map(ox, oy)

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

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

    def planning(self, sx, sy, gx, gy):
        """
        A star path search

        input:
            s_x: start x position [m]
            s_y: start y position [m]
            gx: goal x position [m]
            gy: goal y position [m]

        output:
            rx: x position list of the final path
            ry: y position list of the final path
        """

        start_node = self.Node(self.calc_xy_index(sx, self.min_x),
                               self.calc_xy_index(sy, self.min_y), 0.0, -1)
        goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
                              self.calc_xy_index(gy, self.min_y), 0.0, -1)

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

        while True:
            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

    def calc_final_path(self, goal_node, closed_set):
        # generate final course
        rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [
            self.calc_grid_position(goal_node.y, self.min_y)]
        parent_index = goal_node.parent_index
        while parent_index != -1:
            n = closed_set[parent_index]
            rx.append(self.calc_grid_position(n.x, self.min_x))
            ry.append(self.calc_grid_position(n.y, self.min_y))
            parent_index = n.parent_index

        return rx, ry

    @staticmethod
    def calc_heuristic(n1, n2):
        w = 1.0  # weight of heuristic
        d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
        return d

    def calc_grid_position(self, index, min_position):
        """
        calc grid position

        :param index:
        :param min_position:
        :return:
        """
        pos = index * self.resolution + min_position
        return pos

    def calc_xy_index(self, position, min_pos):
        return round((position - min_pos) / self.resolution)

    def calc_grid_index(self, node):
        return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)

    def verify_node(self, node):
        px = self.calc_grid_position(node.x, self.min_x)
        py = self.calc_grid_position(node.y, self.min_y)

        if px < self.min_x:
            return False
        elif py < self.min_y:
            return False
        elif px >= self.max_x:
            return False
        elif py >= self.max_y:
            return False

        # collision check
        if self.obstacle_map[node.x][node.y]:
            return False

        return True

    def calc_obstacle_map(self, ox, oy):

        self.min_x = round(min(ox))
        self.min_y = round(min(oy))
        self.max_x = round(max(ox))
        self.max_y = round(max(oy))
        print("min_x:", self.min_x)
        print("min_y:", self.min_y)
        print("max_x:", self.max_x)
        print("max_y:", self.max_y)

        self.x_width = round((self.max_x - self.min_x) / self.resolution)
        self.y_width = round((self.max_y - self.min_y) / self.resolution)
        print("x_width:", self.x_width)
        print("y_width:", self.y_width)

        # obstacle map generation
        self.obstacle_map = [[False for _ in range(self.y_width)]
                             for _ in range(self.x_width)]
        for ix in range(self.x_width):
            x = self.calc_grid_position(ix, self.min_x)
            for iy in range(self.y_width):
                y = self.calc_grid_position(iy, self.min_y)
                for iox, ioy in zip(ox, oy):
                    d = math.hypot(iox - x, ioy - y)
                    if d <= self.rr:
                        self.obstacle_map[ix][iy] = True
                        break

    @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


def main():
    print(__file__ + " start!!")

    # start and goal position
    sx = 10.0  # [m]
    sy = 10.0  # [m]
    gx = 50.0  # [m]
    gy = 50.0  # [m]
    grid_size = 2.0  # [m]
    robot_radius = 1.0  # [m]

    # set obstacle 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 = 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")
        plt.pause(0.001)
        plt.show()


if __name__ == '__main__':
    main()
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
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271

希望本章博客对您有帮助