路径规划与轨迹优化 —— A算法_a算法优化

作者：羊村懒王 | 2024-05-07 22:00:00

踩

a*算法优化

一、A*算法的思路

前面提到的Dijkstra算法是一种广度优先搜索算法，它以广度作为优先级，这种特性决定了它在搜索到终点前会尽可能大范围的遍历所有节点。我们运行前文的代码，可以看到广度优先搜索的效果有点像“病毒扩散”。

请添加图片描述

但在实际工程应用中，我们希望减少对节点的收录，并使机器人可以尽快的找到搜索方向。我们的思路是加入一个启发式函数来引导路径的规划。

$f (n) = g (n) + h (n)$
其中

f(n)是节点n的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。
g(n) 是节点n距离起点的代价。
h(n) 是节点n距离终点的代价，这也就是A*算法的启发函数。

加入启发式函数，可以形象的理解为终点的位置对规划产生了一个"引力"，促使着收录节点时更快的向终点进发。所以起点和终点都在影响着A*算法的行为

当启发函数h(n)始终为0，则将由g(n)决定节点的优先级，此时算法就变成了Dijkstra算法。
如果h(n)始终小于等于节点n到终点的代价，则A*算法保证一定能够找到最短路径。但是当h(n)的值越小，算法将遍历越多的节点，也就导致算法越慢。
如果h(n)完全等于节点n到终点的代价，则A*算法将找到最佳路径，并且速度很快。可惜的是，并非所有场景下都能做到这一点。因为在没有达到终点之前，我们很难确切算出距离终点还有多远。
如果h(n)的值比节点n到终点的代价要大，则A*算法不能保证找到最短路径，不过此时会很快。

所以A*算法能找到最短路径，依然保证最优性的条件是

$\leq h(n)_{真实距离}$

A*算法在算法流程上只比Dijkstra算法多加入了启发式函数，其余没有变化

在这里插入图片描述

二、地图距离

补充一些关于地图距离的知识点。不同的地图影响着启发式函数的值。

曼哈顿距离

如果机器人的行走规则中只允许朝上下左右四个方向移动，则使用曼哈顿距离来衡量两个点之间的距离。

def heuristic(node):
    dx = abs(node.x - goal.x)
    dy = abs(node.y - goal.y)
    return D * (dx + dy)  #D是代价
1
2
3
4

在这里插入图片描述
对角距离

如果机器人可以斜向行走，则使用对角距离。计算对角距离的函数如下

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

其中D2是斜向行走的代价，它的计算如下

$\sqrt 2 D$
在这里插入图片描述
欧式距离

欧几里得距离是指两个节点之间的直线距离。计算函数如下

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

三、代码

定义机器人的运动规则有上下左右和斜向方式

    def get_motion_model():    #相当于赋予权重
        # dx, dy, cost（x轴方向，y轴方向，代价（距离））
        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
2
3
4
5
6
7
8
9
10
11
12

因此启发式函数的计算采用对角距离

    #启发式函数的计算采用对角距离
    def calc_heuristic(self,goal_node,current_node):
        D = 1
        D2 = math.sqrt(2) * 1
        dx = abs(goal_node.x - current_node.x)
        dy = abs(goal_node.y - current_node.y)
        return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)
1
2
3
4
5
6
7

完整代码如下：

import matplotlib.pyplot as plt
import math

show_animation = True


class Dijkstra:

    def __init__(self, ox, oy, resolution, robot_radius):
        """
        Initialize map for planning

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

        self.min_x = None
        self.min_y = None
        self.max_x = None
        self.max_y = None
        self.x_width = None
        self.y_width = None
        self.obstacle_map = None

        self.resolution = resolution
        self.robot_radius = robot_radius
        self.calc_obstacle_map(ox, oy)         #构建栅格地图（包含障碍物的膨胀）
        self.motion = self.get_motion_model()  #规定路径权重

    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  # g(n)
            self.parent_index = parent_index  # index of previous Node 前一个结点

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

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

        input:
            s_x: start x position [m]
            s_y: 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
        """
        
        #原始坐标转换成栅格坐标并赋给节点
        start_node = self.Node(self.calc_xy_index(sx, self.min_x),
                               self.calc_xy_index(sy, self.min_y), 0.0, -1)   # round((position - minp) / self.resolution)
        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()     # key - value: hash表

        #calc_index是对节点的一个遍历标号 方便对节点的操作和定位  相当于key
        open_set[self.calc_index(start_node)] = start_node   #起点先加入open_set

        while 1:
            #c_id取的是最小代价点的key
            c_id = min(open_set, key=lambda o: (open_set[o].cost + self.calc_heuristic(goal_node,open_set[o])))  # 取cost最小的节点
            #当前节点
            current = open_set[c_id]

            # show graph
            if show_animation:  # pragma: no cover
                plt.plot(self.calc_position(current.x, self.min_x),
                         self.calc_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 search grid based on motion model
            for move_x, move_y, move_cost in self.motion:
                node = self.Node(current.x + move_x,
                                 current.y + move_y,
                                 current.cost + move_cost, c_id)  #c_id传入的是上一个被收录点的遍历索引
                n_id = self.calc_index(node)

                if n_id in closed_set:  #如果相邻点已经在closed_list 就不用收录了
                    continue

                if not self.verify_node(node):   #符合地图的约束
                    continue

                if n_id not in open_set:   #将新发现的节点收录
                    open_set[n_id] = node  # Discover 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           #更新cost

        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_position(goal_node.x, self.min_x)], [
            self.calc_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_position(n.x, self.min_x))
            ry.append(self.calc_position(n.y, self.min_y))
            parent_index = n.parent_index

        return rx, ry

    #机器人的行走规则
    def calc_heuristic(self,goal_node,current_node):
        D = 1
        D2 = math.sqrt(2) * 1
        dx = abs(goal_node.x - current_node.x)
        dy = abs(goal_node.y - current_node.y)
        return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)

    def calc_position(self, index, minp):
        pos = index * self.resolution + minp
        return pos

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

    def calc_index(self, node):
        return node.y * self.x_width + node.x

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

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

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

        return True

    def calc_obstacle_map(self, ox, oy):
        ''' 第1步：构建栅格地图 '''

        #四个顶点
        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)]
        # 设置障碍物
        # x和y是栅格地图中的坐标  对障碍物做膨胀处理时要找到x和y在于原始地图的坐标
        for ix in range(self.x_width):
            x = self.calc_position(ix, self.min_x)
            for iy in range(self.y_width):
                y = self.calc_position(iy, self.min_y)
                for iox, ioy in zip(ox, oy):
                    d = math.hypot(iox - x, ioy - y)  #原始地图中所有栅格距离障碍物的距离
                    if d <= self.robot_radius:        #障碍物到附近点的距离小于机器人半径 机器人不能通过
                        self.obstacle_map[ix][iy] = True
                        break

    @staticmethod
    def get_motion_model():    #相当于赋予权重
        # dx, dy, cost（x轴方向，y轴方向，代价（距离））
        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():
    # start and goal position

    #起点
    sx = -5.0  # [m]
    sy = -5.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 = [], []     #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")

    #创建Dijkstra对象
    dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)
    rx, ry = dijkstra.planning(sx, sy, gx, gy)

    if show_animation:  # pragma: no cover
        plt.plot(rx, ry, "-r")
        plt.pause(0.01)
        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
272
273
274
275
276
277
278
279
280