凡人多烦事01

这个屌丝很懒，什么也没留下！

热门标签

逐行讲解python实现A路径规划_a算法路径规划python

作者：凡人多烦事01 | 2024-05-01 17:50:43

踩

a*算法路径规划python

搜索步骤

A*路径规划是一种广度优先搜索算法，需要在栅格地图上进行搜索。其主要搜索步骤如下：

得到栅格地图，确定起点和终点位置；
计算起点周围点的代价，放到开集合中，可以周围4个点（四连通），也可以8个点（八连通）；
从开集合中提取代价最小的点，将其放到闭集合中，并计算这个点周围点的代价，此时要判断周围点是否已经在开集合或闭集合中，并做特殊处理（后面详细说明），若不在两个集合中，则将其放到开集合中；
循环执行步骤3，直到弹出的点是终点。

关键点

开集合和闭集合

开集合：待走的位置。相当于候选点集合，每到一个点，将周围点放入候选集合中，往后有可能往候选点去走。
闭集合：已经走过的位置，不会再被考虑。

复杂度优化

上述第三步中 “从开集合中提取代价最小的点” ，常规方法会循环遍历开集合，找到代价值最小的索引来提取，但循环结构太耗费时间。
改进：用小顶堆来解决（一种特殊的数据结构，若不了解可自行百度），小顶堆可以一直保持索引0的点为代价最小的点，它的插入和自更新时间复杂度为O(nlogn)，相对循环更快。
```
import heapq

# 创建一个列表用来存储
open_set = [15]
# 往列表中压入一个新的数，并自行调整小顶堆的顺序
heapq.heappush(open_set, 10)
heapq.heappush(open_set, 20)
heapq.heappush(open_set, 30)
heapq.heappush(open_set, 14)
# 弹出堆顶，永远是最小的数
a = heapq.heappop(open_set)
print(a)
1
2
3
4
5
6
7
8
9
10
11
12
```
上述第三步中 “判断周围点是否已经在开集合或闭集合中”，常规方法会遍历开集合或闭集合，判断该点是否在其中，同样循环太耗费时间。
改进：在创建地图的时候，地图上的每一个点都是一个对象，对应的类中新创建一个属性来记录当前点是否在开集合或闭集合中。用空间换时间，每次判断可直接索引查找，复杂度降维O(1)。
```
# 地图上的每一个点都是一个Point对象，用于记录该点的类别、代价等信息
class Point:
    def __init__(self, x=0, y=0):
        self.x = x
        self.y = y
        self.val = 0  # 0代表可通行，1代表障碍物
        self.cost_g = 0
        self.cost_h = 0
        self.cost_f = 0
        self.parent = None  # 父节点
        self.is_open = 0  # 0：不在开集合  1：在开集合  -1：在闭集合

    # 用于heapq小顶堆的比较
    def __lt__(self, other):
        return self.cost_f < other.cost_f
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
```
注：heapq操作的列表里的元素默认都是数值，可以直接比较大小的，但现在存储的是类对象，所以需要额外写__lt__方法，用于比较大小。
注：父节点，把当前点设为周围扩散点的父节点，用于标识该点来源于哪里，用于回溯完整路径。

代价

代价，即走到某个位置需要花费的成本。代价越小，这条路径就越好。

每个点的代价分为3种：

g 代价：当前点到起点的代价，等于当前点与父节点之间的距离+父节点的 g 代价；
h 代价：当前点到终点的代价，等于当前点与终点之间的距离；
f 代价：当前点总代价，等于g 代价 + h 代价；

父节点替换

当前点在扩散时需要将周围的8个点都放入开集合中（假设8连通），这也意味着某一个点可能由周围8个点扩散过来。

那这个点到底归谁呢（决定它由谁扩散而来），当然是谁代价小就选谁。

对于一个点，它的h值是不变的，永远是与终点的距离，但是它的g值是不一样的，g值跟走过的路径有关，走的越长代价越大，所以选择走的最少的点作为自己的父节点。

所以具体实现步骤如下：

对于扩散到的点，首先判断是否有效（越出边界或障碍物），以及是否已经在闭集合中（已经走过的就不用再走一遍了）
若通过上一步，先将来源点（谁扩散来的）作为父节点，去计算g值（g值需要依靠父节点g值计算）
如果这个点不在开集合中，都好说，直接加入开集合就好
如果在开集合中，说明之前有别的点扩散到这过，比较这两个来源点到这的g值大小，选择小的那个作为自己的父节点。

下图为增加这个操作和不增加的对比，左图如果不增加，它会一直保留第一次扩散过来的点作为自己的父节点，右图切换父节点后就会选择更近的路。
在这里插入图片描述

该部分代码如下

    def diffusion_point(self, x, y, parent):
        # 无效点或者在闭集合中，跳过
        if not self.is_valid_point(x, y) or self.map.map[x][y].is_open == -1:
            return
        p = self.map.map[x][y]
        pre_parent = p.parent
        p.parent = parent
        # 先计算出当前点的总代价
        cost_g = self.g_cost(p)
        cost_h = self.h_cost(p)
        cost_f = cost_g + cost_h
        # 如果在开集合中，判断当前点和开集合中哪个点代价小，换成小的，相同x,y的点h值相同，g值不一定相同
        if p.is_open == 1:
            if cost_f < p.cost_f:
                # 如果从当前parent遍历过来的代价更小，替换成当前的代价和父节点
                p.cost_g, p.cost_h, p.cost_f = cost_g, cost_h, cost_f
            else:
                # 如果从之前父节点遍历过来的代价更小，保持之前的代价和父节点
                p.parent = pre_parent
        else:
            # 如果不在开集合中，说明之间没遍历过，直接加到开集合里就好
            p.cost_g, p.cost_h, p.cost_f = cost_g, cost_h, cost_f
            heapq.heappush(self.open_set, p)
            p.is_open = 1
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

距离

若使用不同距离，可在完整代码中替换对应部分

欧式距离（欧几里得距离）：两个点之间的直线距离

    def g_cost(self, p):
        '''
        计算 g 代价，当前点与父节点的距离 + 父节点的 g 代价（欧氏距离）
        :param p: 当前扩散的节点
        :return: p 的 g 代价
        '''
        x_dis = abs(p.parent.x - p.x)
        y_dis = abs(p.parent.y - p.y)
        return np.sqrt(x_dis ** 2 + y_dis ** 2) + p.parent.cost_g

    def h_cost(self, p):
        '''
        计算 h 代价，当前点与终点之间的距离（欧氏距离）
        :param p: 当前扩散的节点
        :return: p 的 h 代价
        '''
        x_dis = abs(self.end_point.x - p.x)
        y_dis = abs(self.end_point.y - p.y)
        return np.sqrt(x_dis ** 2 + y_dis ** 2)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

曼哈顿距离：计算x轴和y轴之差的绝对值，适合在四连通时使用（只上下左右走），计算简单

    def g_cost(self, p):
        '''
        计算 g 代价，当前点与父节点的距离 + 父节点的 g 代价（曼哈顿距离）
        :param p: 当前扩散的节点
        :return: p 的 g 代价
        '''
        x_dis = abs(p.parent.x - p.x)
        y_dis = abs(p.parent.y - p.y)
        return x_dis + y_dis + p.parent.cost_g

    def h_cost(self, p):
        '''
        计算 h 代价，当前点与终点之间的距离（曼哈顿距离）
        :param p: 当前扩散的节点
        :return: p 的 h 代价
        '''
        x_dis = abs(self.end_point.x - p.x)
        y_dis = abs(self.end_point.y - p.y)
        return x_dis + y_dis
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

对角距离：按照8连通方向走，以最近的方式走到终点所需的距离。是欧式距离与曼哈顿距离的折中版，对角距离只能45度斜着走，而欧式距离可以任意角度。
- 更适合栅格地图的路径规划，大多数情况可无脑使用对角距离。
- 计算方法：如下图所示，对角距离可抽象成1+4，实际计算时公式为 $(1 + 2) + (3) + (4) - 3 - 2$ ，其中 $1 + 2$ 为当前点与终点y轴之差，3为x轴之差， $2 = 3$ ，所以可以简化为 $x + y - 2 * x + s q r t (2) * x$
```
    def g_cost(self, p):
        '''
        计算 g 代价，当前点与父节点的距离 + 父节点的 g 代价（对角距离）
        :param p: 当前扩散的节点
        :return: p 的 g 代价
        '''
        x_dis = abs(p.parent.x - p.x)
        y_dis = abs(p.parent.y - p.y)
    	return np.sqrt(x_dis ** 2 + y_dis ** 2) + p.parent.cost_g

    def h_cost(self, p):
        '''
        计算 h 代价，当前点与终点之间的距离（对角距离）
        :param p: 当前扩散的节点
        :return: p 的 h 代价
        '''
        x_dis = abs(self.end_point.x - p.x)
        y_dis = abs(self.end_point.y - p.y)
        return x_dis + y_dis + (np.sqrt(2) - 2) * min(x_dis, y_dis)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
```

地图设置

创建一个地图类，具体的地图用二维列表存储，每个元素都是Point对象。set_obstacle方法可手动设置障碍物。

class Map:
    def __init__(self, map_size):
        self.map_size = map_size
        self.width = map_size[0]
        self.height = map_size[1]
        self.map = [[Point(x, y) for y in range(self.map_size[1])] for x in range(self.map_size[0])]

    # 手动设置障碍物，可多次调用设置地图
    # 由于地图方向不同，这里的topleft并不总是左上角，topleft代表x和y全都较小的点
    def set_obstacle(self, topleft, width, height):
        for x in range(topleft[0], topleft[0] + width):
            for y in range(topleft[1], topleft[1] + height):
                self.map[x][y].val = 1
1
2
3
4
5
6
7
8
9
10
11
12
13

完整代码

#!/usr/bin/env python
# -*- coding:utf-8 -*-
# @Author      : Cao Zejun
# @Time        : 2024/4/7 17:36
# @File        : Astar_blog.py
# @Software    : Pycharm
# @description : 用于CSDN上的Astar算法演示

import time
import numpy as np
from matplotlib.patches import Rectangle
import matplotlib.pyplot as plt
import heapq


# 地图上的每一个点都是一个Point对象，用于记录该点的类别、代价等信息
class Point:
    def __init__(self, x=0, y=0):
        self.x = x
        self.y = y
        self.val = 0  # 0代表可通行，1代表障碍物
        self.cost_g = 0  # 三个代价
        self.cost_h = 0
        self.cost_f = 0
        self.parent = None  # 父节点
        self.is_open = 0  # 0：不在开集合  1：在开集合  -1：在闭集合

    # 用于heapq小顶堆的比较
    def __lt__(self, other):
        return self.cost_f < other.cost_f


class Map:
    def __init__(self, map_size):
        self.map_size = map_size
        self.width = map_size[0]
        self.height = map_size[1]
        self.map = [[Point(x, y) for y in range(self.map_size[1])] for x in range(self.map_size[0])]

    # 手动设置障碍物，可多次调用设置地图
    # 由于地图方向不同，这里的topleft并不总是左上角，topleft代表x和y全都较小的点
    def set_obstacle(self, topleft, width, height):
        for x in range(topleft[0], topleft[0] + width):
            for y in range(topleft[1], topleft[1] + height):
                self.map[x][y].val = 1


class AStar:
    def __init__(self, map, start_point, end_point, connect_num=8):
        self.map: Map = map
        self.start_point = start_point
        self.end_point = end_point
        self.open_set = [self.start_point]  # 开集合，先放入起点，从起点开始遍历

        self.start_point.is_open = 1  #
        self.connect_num = connect_num  # 连通数，目前支持4连通或8连通
        self.diffuse_dir = [(1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (-1, 1), (1, -1), (-1, -1)]  # 遍历的8个方向，只需取出元组，加到x和y上就可以

    def g_cost(self, p):
        '''
        计算 g 代价，当前点与父节点的距离 + 父节点的 g 代价（欧氏距离）
        :param p: 当前扩散的节点
        :return: p 的 g 代价
        '''
        x_dis = abs(p.parent.x - p.x)
        y_dis = abs(p.parent.y - p.y)
        return np.sqrt(x_dis ** 2 + y_dis ** 2) + p.parent.cost_g

    def h_cost(self, p):
        '''
        计算 h 代价，当前点与终点之间的距离（欧氏距离）
        :param p: 当前扩散的节点
        :return: p 的 h 代价
        '''
        x_dis = abs(self.end_point.x - p.x)
        y_dis = abs(self.end_point.y - p.y)
        return np.sqrt(x_dis ** 2 + y_dis ** 2)

    def is_valid_point(self, x, y):
        # 无效点：超出地图边界或为障碍物
        if x < 0 or x >= self.map.width:
            return False
        if y < 0 or y >= self.map.height:
            return False
        return self.map.map[x][y].val == 0

    def search(self):
        self.start_time = time.time()  # 用于记录搜索时间
        p = self.start_point
        # p 为当前遍历节点，等于终点停下
        while not (p == self.end_point):
            # 弹出代价最小的开集合点，若开集合为空，说明没有路径
            try:
                p = heapq.heappop(self.open_set)
            except:
                raise 'No path found, algorithm failed!!!'

            p.is_open = -1

            # 遍历周围点
            for i in range(self.connect_num):
                dir_x, dir_y = self.diffuse_dir[i]
                self.diffusion_point(p.x + dir_x, p.y + dir_y, p)
        return self.build_path(p)  # p = self.end_point

    def diffusion_point(self, x, y, parent):
        # 无效点或者在闭集合中，跳过
        if not self.is_valid_point(x, y) or self.map.map[x][y].is_open == -1:
            return
        p = self.map.map[x][y]
        pre_parent = p.parent
        p.parent = parent
        # 先计算出当前点的总代价
        cost_g = self.g_cost(p)
        cost_h = self.h_cost(p)
        cost_f = cost_g + cost_h
        # 如果在开集合中，判断当前点和开集合中哪个点代价小，换成小的，相同x,y的点h值相同，g值不一定相同
        if p.is_open == 1:
            if cost_f < p.cost_f:
                # 如果从当前parent遍历过来的代价更小，替换成当前的代价和父节点
                p.cost_g, p.cost_h, p.cost_f = cost_g, cost_h, cost_f
            else:
                # 如果从之前父节点遍历过来的代价更小，保持之前的代价和父节点
                p.parent = pre_parent
        else:
            # 如果不在开集合中，说明之间没遍历过，直接加到开集合里就好
            p.cost_g, p.cost_h, p.cost_f = cost_g, cost_h, cost_f
            heapq.heappush(self.open_set, p)
            p.is_open = 1

    def build_path(self, p):
        print('search time: ', time.time() - self.start_time, ' seconds')
        # 回溯完整路径
        path = []
        while p != self.start_point:
            path.append(p)
            p = p.parent
        print('search time: ', time.time() - self.start_time, ' seconds')
        # 打印开集合、闭集合的数量
        print('open set count: ', len(self.open_set))
        close_count = 0
        for x in range(self.map.width):
            for y in range(self.map.height):
                close_count += 1 if self.map.map[x][y] == -1 else 0
        print('close set count: ', close_count)
        print('total count: ', close_count + len(self.open_set))
        # path = path[::-1]  # path为终点到起点的顺序，可使用该语句翻转
        return path

if __name__ == '__main__':
    map = Map((50, 50))

    # 用于显示plt图
    ax = plt.gca()
    ax.set_xlim([0, map.width])
    ax.set_ylim([0, map.height])
    plt.tight_layout()

    # 设置障碍物
    map.set_obstacle([10, 27], 20, 4)
    # 将障碍物显示到plt上
    ax.add_patch(Rectangle([10, 27], width=20, height=4, color='gray'))

    # 设置起始点和终点，并创建astar对象
    start_point = map.map[5][5]
    end_point = map.map[20][40]
    astar = AStar(map, start_point, end_point)
    path = astar.search()

    # 搜索之后打印，功能拓展时不影响搜索时间
    # 打印开集合点和闭集合点，可视化扩散点数量
    for x in range(map.width):
        for y in range(map.height):
            if map.map[x][y].is_open == -1:
                ax.add_patch(Rectangle([x, y], width=1, height=1, color='green'))
            if map.map[x][y].is_open == 1:
                ax.add_patch(Rectangle([x, y], width=1, height=1, color='blue'))
    # 可视化起点到终点完整路径
    for p in path:
        ax.add_patch(Rectangle([p.x, p.y], width=1, height=1, color='red'))

    # plt.savefig('./output/tmp.jpg')  # 可选择将其保存为本地图片
    plt.show()
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

备注

若有错误，可在评论区指出，我会及时修改

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/凡人多烦事01/article/detail/520016

逐行讲解python实现A*路径规划_a*算法路径规划python

目录