1、理解
Dijkstra算法是路径规划算法中非常经典的一种算法，在很多地方都会用到，特别是在机器人的路径规划中，基本学习机器人运动相关的都会接触到该算法。
Dijkstra算法本身的原理是基于贪心思想实现的，首先把起点到所有点的距离存下来找个最短的，然后松弛一次再找出最短的，所谓的松弛操作就是，遍历一遍看通过刚刚找到的距离最短的点作为中转站会不会更近，
如果更近了就更新距离，这样把所有的点找遍之后就存下了起点到其他所有点的最短距离。
2、代码
1 import math
2 import matplotlib.pyplot as plt
3
4 min_set = 10
5 show_animation = True # 绘图
6
7 # 创建一个类
8
9 class Dijkstra:
10 # 初始化
11 def __init__(self, ox, oy, resolution, robot_radius, resource_points):
12 # 属性分配 设置默认值或初始值
13 self.min_x = None # 地图的最小x坐标
14 self.min_y = None # 地图的最小y坐标
15 self.max_x = None # 地图的最大x坐标
16 self.max_y = None # 地图的最大y坐标
17 self.x_width = None # 地图的x方向网络宽度
18 self.y_width = None # 地图的y方向网格宽度
19 self.obstacle_map = None # 障碍物地图
20
21 # 传递并存储参数
22 self.resolution = resolution # 网格大小(m)
23 self.robot_radius = robot_radius # 机器人半径
24 self.resource_points = resource_points # 资源点
25
26 # 调用初始化方法
27 self.calc_obstacle_map(ox, oy) # 计算障碍物地图
28 self.motion = self.get_motion_model() # 机器人运动方式
29
30 # Node 类定义了路径规划中的一个节点。每个节点包含坐标（x 和 y）、路径成本（cost）以及父节点索引（parent_index）。
31 # 构建节点，每个网格代表一个节点
32 class Node:
33 # 初始化：通过 __init__ 方法初始化节点，确保每个节点都有定义的属性值。
34 def __init__(self, x, y, cost, parent_index):
35 self.x = x # 网格索引
36 self.y = y
37 self.cost = cost # 路径值
38 self.parent_index = parent_index # 该网格的父节点
39
40 # 字符串表示：通过 __str__ 方法定义了节点的字符串表示形式，方便调试和日志记录
41 def __str__(self):
42 return str(self.x) + ',' + str(self.y) + ',' + str(self.cost) + ',' + str(self.parent_index)
43
44 # 寻找最优路径，网格起始坐标(sx,sy)，终点坐标（gx,gy）
45 def planning(self, sx, sy, gx, gy): # planning方法接受起点坐标和终点坐标
46 # 节点初始化
47 # 将已知的起点和终点坐标形式转化为节点类型，0代表路径权重，-1代表无父节点
48 # calc_xy_index方法将实际坐标转换为网格索引，Node的cost初始化为0.0，parent_index初始化为-1，表示起点没有父节点。Node的cost初始化为0.0，parent_index初始化为-1，表示起点没有父节点。
49 start_node = self.Node(self.calc_xy_index(sx, self.min_x),
50 self.calc_xy_index(sy, self.min_y), 0.0, -1)
51 # 终点
52 goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
53 self.calc_xy_index(gy, self.min_y), 0.0, -1)
54
55 # 保存入库节点和待计算节点 使用dict()创建了两个空字典open_set和closed_set。
56 open_set, closed_set = dict(), dict() # 初始化两个集合：open_set用于保存待处理的节点，closed_set用于保存已处理的节点。
57 # 先将起点入库，获取每个网格对应的key
58 # 将起点节点加入到open_set中。self.calc_index(start_node)计算了起点节点的唯一索引值，并将其作为键，将起点节点对象作为值存储在open_set中。
59 open_set[self.calc_index(start_node)] = start_node
60
61 # 循环
62 while 1:
63 # 选择扩展点，添加了启发项，f(n)= g(n) + h(n) 计算了节点的总代价（f值），即实际代价和启发式代价的和。
64 c_id = min(open_set,
65 key=lambda o: open_set[o].cost + \
66 self.calc_heuristic(self,goal_node, open_set[o]))
67 # 选择了 open_set 中代价最小的节点作为当前处理的节点。
68 # 具体来说，这段代码选择了当前待处理节点中具有最小估计总代价（f值）的节点。f值是由当前节点的实际代价g值和启发式代价h值之和得到的。
69 # key=lambda o: ...：这是一个匿名函数（lambda函数），用于计算每个节点的代价。
70 # min()函数将使用这个函数来比较open_set中的每个节点。
71 # min(open_set, key=...):
72 # 这部分代码使用min函数在open_set中查找具有最小代价的节点。min函数的key参数指定了用于比较的准则。
73 # o是open_set的一个键（节点的唯一索引）
74 # open_set[o]是对应于键o的节点对象。
75 # open_set[o].cost是节点的实际代价（从起点到该节点的代价，g值）。
76 # self.calc_heuristic(goal_node, open_set[o])是节点的启发式代价（从该节点到终点的估计代价，h值）。
77
78 current = open_set[c_id] # 从字典中取出该节点
79
80 # 绘图 如果启用了动画，绘制当前节点的位置。
81 if show_animation:
82 # 网格索引转换为真实坐标
83 # self.calc_position(current.y, self.min_y)方法将网格索引转换为实际坐标（单位：米）
84 plt.plot(self.calc_position(current.x, self.min_x),
85 self.calc_position(current.y, self.min_y), 'xc')
86 plt.pause(0.0001) # plt.pause函数可以刷新绘图窗口并暂时停止代码执行
87
88 # 判断是否是终点，如果选出来的损失最小的点是终点
89 if current.x == goal_node.x and current.y == goal_node.y:
90 # 更新终点的父节点
91 goal_node.cost = current.cost
92 # 更新终点的损失
93 goal_node.parent_index = current.parent_index
94 break
95
96 # 在外库中删除该最小损失点，把它入库
97 del open_set[c_id] # 这行代码将当前节点从开放集中删除。
98 closed_set[c_id] = current # 这行代码将当前节点添加到闭合集中。
99 # 避免重复处理
100 # 记录处理过的节点
101 # 确保算法终止：通过不断减少开放集中的节点数量，最终开放集会为空，算法会终止
102
103 # 遍历邻接节点
104 for move_x, move_y, move_cost in self.motion:
105 # self.motion：是一个列表，包含机器人可能的移动方式。
106 # 每个元素都是一个三元组，表示移动的方向（move_x, move_y）和移动的代价（move_cost）。
107
108 # 获取每个邻接节点的节点坐标，到起点的距离，父节点
109 node = self.Node(current.x + move_x,
110 current.y + move_y,
111 current.cost + move_cost, c_id)
112
113 # current.x + move_x, current.y + move_y 计算邻接节点的坐标
114 # current.cost + move_cost：计算从起点到邻接节点的代价
115 # c_id：当前节点的索引，作为邻接节点的父节点索引
116
117 # 获取该邻居节点的key
118 n_id = self.calc_index(node)
119 # 计算邻接节点的唯一索引，用于在开放集和闭合集中进行查找
120
121 # 如果该节点入库了，就看下一个
122 # 如果邻接节点已经在闭合集中，跳过这个节点（因为它已经被处理过）。
123 if n_id in closed_set:
124 continue
125
126 # 邻居节点是否超出范围了，是否在障碍物上
127 if not self.verify_node(node):
128 continue
129
130 # 如果该节点不在外库中，就作为一个新节点加入到外库
131 if n_id not in open_set:
132 open_set[n_id] = node
133 # 节点在外库中时
134 else:
135 # 如果该点到起点的距离，要小于外库中该点的距离，就更新外库中的该点信息，更改路径
136 # 如果邻接节点已经在开放集中，检查新的路径代价是否小于当前路径代价。
137 # 如果新的路径代价较小，更新开放集中节点的信息，表示找到了更优的路径。
138 if node.cost <= open_set[n_id].cost:
139 open_set[n_id] = node
140
141 # 找到终点
142 rx, ry = self.calc_final_path(goal_node, closed_set)
143 return rx, ry
144
145 # ------------------------------ #
146 # A* 的启发函数
147 # ------------------------------ #
148 @staticmethod
149 def calc_heuristic(self,n1, n2): # n1终点，n2当前网格
150 w = 1.0 # 单个启发函数的权重，如果有多个启发函数，权重可以设置的不一样
151 d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) # 当前网格和终点距离
152 resource_value = 0
153 for rx, ry in self.resource_points:
154 if n2.x == self.calc_xy_index(rx, self.min_x) and n2.y == self.calc_xy_index(ry, self.min_y):
155 resource_value += 1
156 return d - resource_value
157
158 # return d
159 # 这一行计算两个点 n1 和 n2 之间的欧几里得距离，并乘以权重 w。
160 # math.hypot 函数计算两个点在平面上的直线距离，公式为 sqrt((n1.x - n2.x)^2 + (n1.y - n2.y)^2)。
161
162 # 机器人行走的方式，每次能向周围移动8个网格移动
163 @staticmethod
164 def get_motion_model():
165 # [dx, dy, cost]
166 motion = [[1, 0, 1], # 右
167 [0, 1, 1], # 上
168 [-1, 0, 1], # 左
169 [0, -1, 1], # 下
170 [-1, -1, math.sqrt(2)], # 左下
171 [-1, 1, math.sqrt(2)], # 左上
172 [1, -1, math.sqrt(2)], # 右下
173 [1, 1, math.sqrt(2)]] # 右上
174 return motion
175
176 # 绘制栅格地图
177 def calc_obstacle_map(self, ox, oy):
178 # 地图边界坐标
179 self.min_x = round(min(ox)) # 四舍五入取整
180 self.min_y = round(min(oy))
181 self.max_x = round(max(ox))
182 self.max_y = round(max(oy))
183 # 地图的x和y方向的栅格个数，长度/每个网格的长度=网格个数
184 self.x_width = round((self.max_x - self.min_x) / self.resolution) # x方向网格个数
185 self.y_width = round((self.max_y - self.min_y) / self.resolution) # y方向网格个数
186 # 初始化地图，二维列表，每个网格的值为False
187 self.obstacle_map = [[False for _ in range(self.y_width)]
188 for _ in range(self.x_width)]
189 # 这一行代码初始化地图为一个二维列表，地图中每个网格的初始值为 False，表示没有障碍物
190 # [[False for _ in range(self.y_width)] for _ in range(self.x_width)] 是一个嵌套的列表推导式，用于生成二维列表。
191 # 外层列表推导式 for _ in range(self.x_width) 迭代 self.x_width 次，每次生成一行（即一个内层列表）。
192 # 内层列表推导式 for _ in range(self.y_width) 迭代 self.y_width 次，每次生成一个 False 值。
193 # 变量 _ 通常用于表示一个不需要使用的循环变量。在这种情况下，我们只关心生成的列表长度，而不关心循环变量的具体值。
194
195 # 设置障碍物
196 for ix in range(self.x_width): # 遍历x方向的网格 [0:x_width]
197 x = self.calc_position(ix, self.min_x) # 根据网格索引计算x坐标位置
198 # 调用 self.calc_position 方法，根据当前网格索引 ix 和最小x值 self.min_x 计算出该网格在地图上的实际x坐标位置。
199 for iy in range(self.y_width): # 遍历y方向的网格 [0:y_width]
200 y = self.calc_position(iy, self.min_y) # 根据网格索引计算y坐标位置
201 # 调用 self.calc_position 方法，根据当前网格索引 iy 和最小y值 self.min_y 计算出该网格在地图上的实际y坐标位置。
202 # 遍历障碍物坐标(实际坐标)
203 for iox, ioy in zip(ox, oy):
204 # 计算障碍物和网格点之间的距离
205 d = math.hypot(iox - x, ioy - y)
206
207 # 膨胀障碍物，如果障碍物和网格之间的距离小，机器人无法通行，对障碍物膨胀
208 if d <= self.robot_radius + self.resolution: # 膨胀范围增加
209 # 将障碍物所在网格设置为True
210 self.obstacle_map[ix][iy] = True
211 break # 每个障碍物膨胀一次就足够了
212
213 # 根据网格编号计算实际坐标
214 def calc_position(self, index, minp):
215 # minp代表起点坐标，左下x或左下y
216 pos = minp + index * self.resolution # 网格点左下左下坐标
217 return pos
218
219 # 位置坐标转为网格坐标
220 def calc_xy_index(self, position, minp):
221 # (目标位置坐标-起点坐标)/一个网格的长度==>目标位置的网格索引
222 return round((position - minp) / self.resolution)
223
224 # 给每个网格编号，得到每个网格的key
225 def calc_index(self, node):
226 # 从左到右增大，从下到上增大
227 return node.y * self.x_width + node.x
228
229 # 邻居节点是否超出范围
230 def verify_node(self, node):
231 # 根据网格坐标计算实际坐标
232 px = self.calc_position(node.x, self.min_x)
233 py = self.calc_position(node.y, self.min_y)
234 # 判断是否超出边界
235 if px < self.min_x:
236 return False
237 if py < self.min_y:
238 return False
239 if px >= self.max_x:
240 return False
241 if py >= self.max_y:
242 return False
243 # 节点是否在障碍物上，障碍物标记为True
244 if self.obstacle_map[node.x][node.y]:
245 return False
246 # 没超过就返回True
247 return True
248
249 # 计算路径, parent属性记录每个节点的父节点
250 def calc_final_path(self, goal_node, closed_set):
251 # 先存放终点坐标（真实坐标）
252 rx = [self.calc_position(goal_node.x, self.min_x)]
253 ry = [self.calc_position(goal_node.y, self.min_y)]
254 # 获取终点的父节点索引
255 parent_index = goal_node.parent_index
256 # 起点的父节点==-1
257 while parent_index != -1:
258 n = closed_set[parent_index] # 在入库中选择父节点
259 rx.append(self.calc_position(n.x, self.min_x)) # 节点的x坐标
260 ry.append(self.calc_position(n.y, self.min_y)) # 节点的y坐标
261 parent_index = n.parent_index # 节点的父节点索引
262
263 return rx, ry
264
265 def draw_circle(cx, cy, radius):
266 ox, oy = [], []
267 for angle in range(0, 360, 5): # 每隔5度取一个点
268 rad = math.radians(angle)
269 x = cx + radius * math.cos(rad)
270 y = cy + radius * math.sin(rad)
271 ox.append(x)
272 oy.append(y)
273 return ox, oy
274
275 def main():
276 # 设置起点和终点
277 # sx = 10
278 sx = -5.0
279 sy = -5.0
280 gx = 50.0
281 gy = 39.0
282 # 网格大小
283 grid_size = 2.0
284 # 机器人半径
285 robot_radius = 1.0
286
287 resource_points = [(-5,10),(10,30),(10,40)]
288
289 # 设置障碍物位置
290 # 边界
291 ox, oy = [], []
292 for i in range(-10, 60):
293 ox.append(i)
294 oy.append(-10.0) # 下边界
295 for i in range(-10, 60):
296 ox.append(60.0)
297 oy.append(i) # 右边界
298 for i in range(-10, 61):
299 ox.append(i)
300 oy.append(60.0) # 上边界
301 for i in range(-10, 61):
302 ox.append(-10.0)
303 oy.append(i) # 左边界
304
305 # 添加圆形障碍物
306 cx, cy, radius = 30, 30, 10
307 circle_ox, circle_oy = draw_circle(cx, cy, radius)
308 ox.extend(circle_ox)
309 oy.extend(circle_oy)
310
311 cx1, cy1, radius1 = 10, 10, 5
312 circle_ox1, circle_oy1 = draw_circle(cx1, cy1, radius1)
313 ox.extend(circle_ox1)
314 oy.extend(circle_oy1)
315
316 # 添加围栏
317 for i in range(-10, 15):
318 ox.append(20.0)
319 oy.append(i) # 左围栏
320
321 # 绘图
322 if show_animation:
323 plt.plot(ox, oy, '.k') # 障碍物黑色
324 plt.plot(sx, sy, 'og') # 起点绿色
325 plt.plot(gx, gy, 'xb') # 终点蓝色
326 plt.grid(True)
327 plt.axis('equal') # 坐标轴刻度间距等长
328 for rx, ry in resource_points:
329 plt.plot(rx, ry, '*r')
330
331 # 实例化，传入障碍物，网格大小
332 dijkstra = Dijkstra(ox, oy, grid_size, robot_radius, resource_points)
333 # 求解路径，返回路径的 x 坐标和 y 坐标列表
334 rx, ry = dijkstra.planning(sx, sy, gx, gy)
335
336 # 绘制路径经过的网格
337 if show_animation:
338 plt.plot(rx, ry, '-r')
339 plt.pause(0.01)
340 plt.show()
341
342 if __name__ == '__main__':
343 main()
（2）增加一个途经点，分段规划
1 import math
2 import matplotlib.pyplot as plt
3
4 min_set = 10
5 show_animation = True # 绘图
6
7 # 创建一个类
8
9 class Dijkstra:
10 # 初始化
11 def __init__(self, ox, oy, resolution, robot_radius, resource_points):
12 # 属性分配 设置默认值或初始值
13 self.min_x = None # 地图的最小x坐标
14 self.min_y = None # 地图的最小y坐标
15 self.max_x = None # 地图的最大x坐标
16 self.max_y = None # 地图的最大y坐标
17 self.x_width = None # 地图的x方向网络宽度
18 self.y_width = None # 地图的y方向网格宽度
19 self.obstacle_map = None # 障碍物地图
20
21 # 传递并存储参数
22 self.resolution = resolution # 网格大小(m)
23 self.robot_radius = robot_radius # 机器人半径
24 self.resource_points = resource_points # 资源点
25
26 # 调用初始化方法
27 self.calc_obstacle_map(ox, oy) # 计算障碍物地图
28 self.motion = self.get_motion_model() # 机器人运动方式
29
30 # Node 类定义了路径规划中的一个节点。每个节点包含坐标（x 和 y）、路径成本（cost）以及父节点索引（parent_index）。
31 # 构建节点，每个网格代表一个节点
32 class Node:
33 # 初始化：通过 __init__ 方法初始化节点，确保每个节点都有定义的属性值。
34 def __init__(self, x, y, cost, parent_index):
35 self.x = x # 网格索引
36 self.y = y
37 self.cost = cost # 路径值
38 self.parent_index = parent_index # 该网格的父节点
39
40 # 字符串表示：通过 __str__ 方法定义了节点的字符串表示形式，方便调试和日志记录
41 def __str__(self):
42 return str(self.x) + ',' + str(self.y) + ',' + str(self.cost) + ',' + str(self.parent_index)
43
44 # 寻找最优路径，网格起始坐标(sx,sy)，终点坐标（gx,gy）
45 def planning(self, sx, sy, gx, gy): # planning方法接受起点坐标和终点坐标
46 # 节点初始化
47 # 将已知的起点和终点坐标形式转化为节点类型，0代表路径权重，-1代表无父节点
48 # calc_xy_index方法将实际坐标转换为网格索引，Node的cost初始化为0.0，parent_index初始化为-1，表示起点没有父节点。Node的cost初始化为0.0，parent_index初始化为-1，表示起点没有父节点。
49 start_node = self.Node(self.calc_xy_index(sx, self.min_x),
50 self.calc_xy_index(sy, self.min_y), 0.0, -1)
51 # 终点
52 goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
53 self.calc_xy_index(gy, self.min_y), 0.0, -1)
54
55 # 保存入库节点和待计算节点 使用dict()创建了两个空字典open_set和closed_set。
56 open_set, closed_set = dict(), dict() # 初始化两个集合：open_set用于保存待处理的节点，closed_set用于保存已处理的节点。
57 # 先将起点入库，获取每个网格对应的key
58 # 将起点节点加入到open_set中。self.calc_index(start_node)计算了起点节点的唯一索引值，并将其作为键，将起点节点对象作为值存储在open_set中。
59 open_set[self.calc_index(start_node)] = start_node
60
61 # 循环
62 while 1:
63 # 选择扩展点，添加了启发项，f(n)= g(n) + h(n) 计算了节点的总代价（f值），即实际代价和启发式代价的和。
64 c_id = min(open_set,
65 key=lambda o: open_set[o].cost + \
66 self.calc_heuristic(self,goal_node, open_set[o]))
67 # 选择了 open_set 中代价最小的节点作为当前处理的节点。
68 # 具体来说，这段代码选择了当前待处理节点中具有最小估计总代价（f值）的节点。f值是由当前节点的实际代价g值和启发式代价h值之和得到的。
69 # key=lambda o: ...：这是一个匿名函数（lambda函数），用于计算每个节点的代价。
70 # min()函数将使用这个函数来比较open_set中的每个节点。
71 # min(open_set, key=...):
72 # 这部分代码使用min函数在open_set中查找具有最小代价的节点。min函数的key参数指定了用于比较的准则。
73 # o是open_set的一个键（节点的唯一索引）
74 # open_set[o]是对应于键o的节点对象。
75 # open_set[o].cost是节点的实际代价（从起点到该节点的代价，g值）。
76 # self.calc_heuristic(goal_node, open_set[o])是节点的启发式代价（从该节点到终点的估计代价，h值）。
77
78 current = open_set[c_id] # 从字典中取出该节点
79
80 # 绘图 如果启用了动画，绘制当前节点的位置。
81 if show_animation:
82 # 网格索引转换为真实坐标
83 # self.calc_position(current.y, self.min_y)方法将网格索引转换为实际坐标（单位：米）
84 plt.plot(self.calc_position(current.x, self.min_x),
85 self.calc_position(current.y, self.min_y), 'xc')
86 plt.pause(0.0001) # plt.pause函数可以刷新绘图窗口并暂时停止代码执行
87
88 # 判断是否是终点，如果选出来的损失最小的点是终点
89 if current.x == goal_node.x and current.y == goal_node.y:
90 # 更新终点的父节点
91 goal_node.cost = current.cost
92 # 更新终点的损失
93 goal_node.parent_index = current.parent_index
94 break
95
96 # 在外库中删除该最小损失点，把它入库
97 del open_set[c_id] # 这行代码将当前节点从开放集中删除。
98 closed_set[c_id] = current # 这行代码将当前节点添加到闭合集中。
99 # 避免重复处理
100 # 记录处理过的节点
101 # 确保算法终止：通过不断减少开放集中的节点数量，最终开放集会为空，算法会终止
102
103 # 遍历邻接节点
104 for move_x, move_y, move_cost in self.motion:
105 # self.motion：是一个列表，包含机器人可能的移动方式。
106 # 每个元素都是一个三元组，表示移动的方向（move_x, move_y）和移动的代价（move_cost）。
107
108 # 获取每个邻接节点的节点坐标，到起点的距离，父节点
109 node = self.Node(current.x + move_x,
110 current.y + move_y,
111 current.cost + move_cost, c_id)
112
113 # current.x + move_x, current.y + move_y 计算邻接节点的坐标
114 # current.cost + move_cost：计算从起点到邻接节点的代价
115 # c_id：当前节点的索引，作为邻接节点的父节点索引
116
117 # 获取该邻居节点的key
118 n_id = self.calc_index(node)
119 # 计算邻接节点的唯一索引，用于在开放集和闭合集中进行查找
120
121 # 如果该节点入库了，就看下一个
122 # 如果邻接节点已经在闭合集中，跳过这个节点（因为它已经被处理过）。
123 if n_id in closed_set:
124 continue
125
126 # 邻居节点是否超出范围了，是否在障碍物上
127 if not self.verify_node(node):
128 continue
129
130 # 如果该节点不在外库中，就作为一个新节点加入到外库
131 if n_id not in open_set:
132 open_set[n_id] = node
133 # 节点在外库中时
134 else:
135 # 如果该点到起点的距离，要小于外库中该点的距离，就更新外库中的该点信息，更改路径
136 # 如果邻接节点已经在开放集中，检查新的路径代价是否小于当前路径代价。
137 # 如果新的路径代价较小，更新开放集中节点的信息，表示找到了更优的路径。
138 if node.cost <= open_set[n_id].cost:
139 open_set[n_id] = node
140
141 # 找到终点
142 rx, ry = self.calc_final_path(goal_node, closed_set)
143 return rx, ry
144
145 # ------------------------------ #
146 # A* 的启发函数
147 # ------------------------------ #
148 @staticmethod
149 def calc_heuristic(self,n1, n2): # n1终点，n2当前网格
150 w = 1.0 # 单个启发函数的权重，如果有多个启发函数，权重可以设置的不一样
151 d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) # 当前网格和终点距离
152 resource_value = 0
153 for rx, ry in self.resource_points:
154 if n2.x == self.calc_xy_index(rx, self.min_x) and n2.y == self.calc_xy_index(ry, self.min_y):
155 resource_value += 1
156 return d - resource_value
157
158 # return d
159 # 这一行计算两个点 n1 和 n2 之间的欧几里得距离，并乘以权重 w。
160 # math.hypot 函数计算两个点在平面上的直线距离，公式为 sqrt((n1.x - n2.x)^2 + (n1.y - n2.y)^2)。
161
162 # 机器人行走的方式，每次能向周围移动8个网格移动
163 @staticmethod
164 def get_motion_model():
165 # [dx, dy, cost]
166 motion = [[1, 0, 1], # 右
167 [0, 1, 1], # 上
168 [-1, 0, 1], # 左
169 [0, -1, 1], # 下
170 [-1, -1, math.sqrt(2)], # 左下
171 [-1, 1, math.sqrt(2)], # 左上
172 [1, -1, math.sqrt(2)], # 右下
173 [1, 1, math.sqrt(2)]] # 右上
174 return motion
175
176 # 绘制栅格地图
177 def calc_obstacle_map(self, ox, oy):
178 # 地图边界坐标
179 self.min_x = round(min(ox)) # 四舍五入取整
180 self.min_y = round(min(oy))
181 self.max_x = round(max(ox))
182 self.max_y = round(max(oy))
183 # 地图的x和y方向的栅格个数，长度/每个网格的长度=网格个数
184 self.x_width = round((self.max_x - self.min_x) / self.resolution) # x方向网格个数
185 self.y_width = round((self.max_y - self.min_y) / self.resolution) # y方向网格个数
186 # 初始化地图，二维列表，每个网格的值为False
187 self.obstacle_map = [[False for _ in range(self.y_width)]
188 for _ in range(self.x_width)]
189 # 这一行代码初始化地图为一个二维列表，地图中每个网格的初始值为 False，表示没有障碍物
190 # [[False for _ in range(self.y_width)] for _ in range(self.x_width)] 是一个嵌套的列表推导式，用于生成二维列表。
191 # 外层列表推导式 for _ in range(self.x_width) 迭代 self.x_width 次，每次生成一行（即一个内层列表）。
192 # 内层列表推导式 for _ in range(self.y_width) 迭代 self.y_width 次，每次生成一个 False 值。
193 # 变量 _ 通常用于表示一个不需要使用的循环变量。在这种情况下，我们只关心生成的列表长度，而不关心循环变量的具体值。
194
195 # 设置障碍物
196 for ix in range(self.x_width): # 遍历x方向的网格 [0:x_width]
197 x = self.calc_position(ix, self.min_x) # 根据网格索引计算x坐标位置
198 # 调用 self.calc_position 方法，根据当前网格索引 ix 和最小x值 self.min_x 计算出该网格在地图上的实际x坐标位置。
199 for iy in range(self.y_width): # 遍历y方向的网格 [0:y_width]
200 y = self.calc_position(iy, self.min_y) # 根据网格索引计算y坐标位置
201 # 调用 self.calc_position 方法，根据当前网格索引 iy 和最小y值 self.min_y 计算出该网格在地图上的实际y坐标位置。
202 # 遍历障碍物坐标(实际坐标)
203 for iox, ioy in zip(ox, oy):
204 # 计算障碍物和网格点之间的距离
205 d = math.hypot(iox - x, ioy - y)
206
207 # 膨胀障碍物，如果障碍物和网格之间的距离小，机器人无法通行，对障碍物膨胀
208 if d <= self.robot_radius + self.resolution: # 膨胀范围增加
209 # 将障碍物所在网格设置为True
210 self.obstacle_map[ix][iy] = True
211 break # 每个障碍物膨胀一次就足够了
212
213 # 根据网格编号计算实际坐标
214 def calc_position(self, index, minp):
215 # minp代表起点坐标，左下x或左下y
216 pos = minp + index * self.resolution # 网格点左下左下坐标
217 return pos
218
219 # 位置坐标转为网格坐标
220 def calc_xy_index(self, position, minp):
221 # (目标位置坐标-起点坐标)/一个网格的长度==>目标位置的网格索引
222 return round((position - minp) / self.resolution)
223
224 # 给每个网格编号，得到每个网格的key
225 def calc_index(self, node):
226 # 从左到右增大，从下到上增大
227 return node.y * self.x_width + node.x
228
229 # 邻居节点是否超出范围
230 def verify_node(self, node):
231 # 根据网格坐标计算实际坐标
232 px = self.calc_position(node.x, self.min_x)
233 py = self.calc_position(node.y, self.min_y)
234 # 判断是否超出边界
235 if px < self.min_x:
236 return False
237 if py < self.min_y:
238 return False
239 if px >= self.max_x:
240 return False
241 if py >= self.max_y:
242 return False
243 # 节点是否在障碍物上，障碍物标记为True
244 if self.obstacle_map[node.x][node.y]:
245 return False
246 # 没超过就返回True
247 return True
248
249 # 计算路径, parent属性记录每个节点的父节点
250 def calc_final_path(self, goal_node, closed_set):
251 # 先存放终点坐标（真实坐标）
252 rx = [self.calc_position(goal_node.x, self.min_x)]
253 ry = [self.calc_position(goal_node.y, self.min_y)]
254 # 获取终点的父节点索引
255 parent_index = goal_node.parent_index
256 # 起点的父节点==-1
257 while parent_index != -1:
258 n = closed_set[parent_index] # 在入库中选择父节点
259 rx.append(self.calc_position(n.x, self.min_x)) # 节点的x坐标
260 ry.append(self.calc_position(n.y, self.min_y)) # 节点的y坐标
261 parent_index = n.parent_index # 节点的父节点索引
262
263 return rx, ry
264
265 def draw_circle(cx, cy, radius):
266 ox, oy = [], []
267 for angle in range(0, 360, 5): # 每隔5度取一个点
268 rad = math.radians(angle)
269 x = cx + radius * math.cos(rad)
270 y = cy + radius * math.sin(rad)
271 ox.append(x)
272 oy.append(y)
273 return ox, oy
274
275 def main():
276 # 设置起点和终点
277 # sx = 10
278 sx = -5.0
279 sy = -5.0
280 gx = 50.0
281 gy = 39.0
282 # 网格大小
283 grid_size = 2.0
284 # 机器人半径
285 robot_radius = 1.0
286
287 resource_points = [(-5,10),(10,30),(10,40)]
288 resource_x = 10
289 resource_y = 30
290
291
292 # 设置障碍物位置
293 # 边界
294 ox, oy = [], []
295 for i in range(-10, 60):
296 ox.append(i)
297 oy.append(-10.0) # 下边界
298 for i in range(-10, 60):
299 ox.append(60.0)
300 oy.append(i) # 右边界
301 for i in range(-10, 61):
302 ox.append(i)
303 oy.append(60.0) # 上边界
304 for i in range(-10, 61):
305 ox.append(-10.0)
306 oy.append(i) # 左边界
307
308 # 添加圆形障碍物
309 cx, cy, radius = 30, 30, 10
310 circle_ox, circle_oy = draw_circle(cx, cy, radius)
311 ox.extend(circle_ox)
312 oy.extend(circle_oy)
313
314 cx1, cy1, radius1 = 10, 10, 5
315 circle_ox1, circle_oy1 = draw_circle(cx1, cy1, radius1)
316 ox.extend(circle_ox1)
317 oy.extend(circle_oy1)
318
319 # 添加围栏
320 for i in range(-10, 15):
321 ox.append(20.0)
322 oy.append(i) # 左围栏
323
324 # 绘图
325 if show_animation:
326 plt.plot(ox, oy, '.k') # 障碍物黑色
327 plt.plot(sx, sy, 'og') # 起点绿色
328 plt.plot(gx, gy, 'xb') # 终点蓝色
329 plt.grid(True)
330 plt.axis('equal') # 坐标轴刻度间距等长
331 for rx, ry in resource_points:
332 plt.plot(rx, ry, '*r')
333
334 # 实例化，传入障碍物，网格大小
335 dijkstra = Dijkstra(ox, oy, grid_size, robot_radius, resource_points)
336 # 求解路径，返回路径的 x 坐标和 y 坐标列表
337 rx1, ry1 = dijkstra.planning(sx, sy, resource_x, resource_y)
338 rx2, ry2 = dijkstra.planning(resource_x,resource_y,gx,gy)
339
340 # 合并路径
341 rx = rx1[:-1] + rx2
342 ry = ry1[:-1] + ry2
343
344
345 # 绘制路径经过的网格
346 if show_animation:
347 plt.plot(rx1, ry1, '-r')
348 plt.plot(rx2, ry2, '-r')
349 plt.pause(0.01)
350 plt.show()
351
352 if __name__ == '__main__':
353 main()
