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()
标签:node,index,set,min,self,算法,Dijkstra,无人机,节点 From: https://www.cnblogs.com/Zhouce/p/18283354