赞
踩
提示:前文写了D搜索算法,是一种贪心算法。
D*算法也是用于机器人路径规划问题的启发式方法,它是一种局部规划方法,即仅仅已知一部分地形,对地形的未知部分进行假设,并在这些假设下找到当前坐标到目标坐标的最短路径。然后机器人沿着这条路走,当它观察到新的地图信息(如从前未知的障碍)时,将这些信息添加到地图中,并在必要时重新规划从当前坐标到给定目标坐标的新的最短路径。重复这个过程,直到达到目标坐标或无法达到目标坐标。

如上图的空间,给定起点(绿色点)、目标点(G蓝色)、障碍(红色),如何进行路径规划?





# coding=utf-8 import matplotlib.pyplot as plt import numpy as np import math map_grid = [[1 for j in range(0, 8)] for i in range(0, 8)] # 定义列表 map_grid = np.array(map_grid) # 将列表转化为数组,因为只有数组才有维度的概念,方便切片 map_grid[3:6, 1] = 0 # 障碍物 map_grid[3:6, 5] = 0 map_grid[0, 3] = 5 # 起点 map_grid[7, 3] = 6 # 终点 def draw_effect(map_grid,second_path): plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10) # 绘制热力图 plt.colorbar() plt.xlim(-1, 8) # x轴的范围 plt.ylim(-1, 8) my_x_ticks = np.arange(0, 8, 1) # x轴标号的范围 my_y_ticks = np.arange(0, 8, 1) plt.xticks(my_x_ticks) plt.yticks(my_y_ticks) second_path = np.array(second_path) plt.plot(second_path[:, 1:2],second_path[:, 0:1],'-') # plt.grid(True) # 开启栅格 可以不开启 plt.show() # 可视化 open_list = [[7, 3, 0, 0, None, None]] # 将终点置于open列表中列表中分别有x0,y0坐标,h值,父节点X、Y坐标 close_list = [] # draw_effect(map_grid) # 将邻域放入open_list中 def open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list): if 0 <= x1 <= 7 and 0 <= y1 <= 7 and map_grid[x1, y1] != 4 and map_grid[x1, y1] != 0: # 左边没有越界并且没有在closelist里面 if map_grid[x1, y1] == 3: # 如果是在open_list中,h要更新 open_list = np.array(open_list) if (h1 + h0) < open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 2]: h = h1 + h0 k = h1 + h0 open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 2] = h open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 3] = k open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 4] = x0 open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 4] = y0 open_list = list(open_list.tolist()) else: # 是new节点 h = h1 + h0 k = h1 + h0 # open_list = list(open_list) open_list.append([x1, y1, h, k, x0, y0]) map_grid[x1, y1] = 3 return open_list # 首次搜索 def first_search(open_list, close_list, map_grid): # 给出终点坐标,完成首次遍历 # 采用D算法遍历 # 选openlist中h最小的,将openlist按照h排序,取第一个,并删除第一个,将它放到close_list里面 open_list = list(open_list) open_list.sort(key=lambda x: x[2]) # open_list.pop(0) insert_list = open_list[0] # 引入中间列表,用来存储每一次被选中的遍历的点 x0 = int(insert_list[0]) y0 = int(insert_list[1]) open_list.pop(0) close_list.append(list(insert_list)) map_grid[x0, y0] = 4 # 被加入到close_list里面 # 找insert_list的邻域 ----->寻找顺序:从左边开始逆时针 h0 = int(insert_list[2]) x1 = x0 y1 = y0 - 1 h1 = 10 open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list) x1 = x0 - 1 y1 = y0 - 1 h1 = 14 open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list) x1 = x0 - 1 y1 = y0 h1 = 10 open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list) x1 = x0 - 1 y1 = y0 + 1 h1 = 14 open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list) x1 = x0 y1 = y0 + 1 h1 = 10 open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list) x1 = x0 + 1 y1 = y0 + 1 h1 = 14 open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list) x1 = x0 + 1 y1 = y0 h1 = 10 open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list) x1 = x0 + 1 y1 = y0 - 1 h1 = 14 open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list) return [open_list, close_list, map_grid] while map_grid[0, 3] != 4 and open_list != []: [open_list, close_list, map_grid] = first_search(open_list, close_list, map_grid) # 首次搜索完成 first_path = [] close_list = np.array(close_list) xn = 0 yn = 3 while xn != 7 or yn != 3: list1 = list(close_list[np.where((close_list[:, 0] == xn) & (close_list[:, 1] == yn))][0]) xn = int(list1[4]) yn = int(list1[5]) first_path.append(list1) first_path.append([7, 3, 0, 0, None, None])

没开始搜索前的地图

第一步搜索图
# 通过上面的程序已经找到了一条路径,完成了第一次的搜索,此时每个节点的h和k是相等的。此时开始点在close list里面,最短路径在firstpath中。 # 可以看出,每个节点的父节点都是该节点的八个邻节点中k值最小的哪个。 # 当出现动态变化时,我们可以利用这个图尽快修正我们的路径,而不是重新规划。 # 当我们检测到某点被阻碍了:1、修改这个点的h值,h变为inf,把它放入openlist中。注意此时该节点的k还是小值,是原来哪个h的值,因此它将立即被取出 # 2、把这个修改扩散出去,直到kmin >=h # 设置一个突然出现的障碍 map_grid[3, 3] = 0 close_list[np.where((close_list[:, 4] == 3) & (close_list[:, 5] == 3)), 2] = math.inf close_list[np.where((close_list[:, 0] == 3) & (close_list[:, 1] == 3)), 2] = math.inf insertRow = list(close_list[np.where((close_list[:, 4] == 3) & (close_list[:, 5] == 3))][0]) x = int(insertRow[0]) y = int(insertRow[1]) open_list.append(insertRow) # ->>>>>>open_list是列表格式 map_grid[x, y] = 3 close_list = list(close_list.tolist()) # ----->>>>>close_list是列表格式 close_list.remove(insertRow) open_list.sort(key=lambda x: x[3]) # 先排序,选择k最小的节点 k_min = open_list[0][3] # hx = open_list[0][2] # 接下来把这个点扩散出去 def find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list): close_list = np.array(close_list) hy = close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2][0] if (hy <= k_old) and (hx > hy + h1): close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 4] = x1 close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 5] = y1 close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 2] = hy + h1 hx = hy + h1 return [hx, list(close_list.tolist())] def find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, map_grid): close_list = np.array(close_list) hy = close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2][0] if (close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] == x0 and close_list[ np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] == y0 and (hy != hx + h1)) or ( (hy > hx + h1) and ( (close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] != x0) or ( close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] != y0))): close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] = x0 close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] = y0 close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2] = hx + h1 Y = list(close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1))][0]) # 把Y放入open_list中 close_list = list(close_list.tolist()) close_list.remove(Y) open_list.append(Y) map_grid[x1, y1] = 3 return [open_list, close_list, map_grid] def find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, map_grid): close_list = np.array(close_list) hy = close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2][0] if (close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] == x0 and close_list[ np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] == y0 and (hy != hx + h1)): close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] = x0 close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] = y0 close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2] = hx + h1 Y = list(close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1))][0]) # 把Y放入open_list中 close_list = list(close_list.tolist()) close_list.remove(Y) open_list.append(Y) map_grid[x1, y1] = 3 else: if ((close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] != x0 or close_list[ np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] != y0) and (hy > hx + h1)): #print(list(close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0))][0])) if map_grid[x0,y0]!=3: X = list(close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0))][0]) close_list = list(close_list.tolist()) close_list.remove(X) open_list.append(X) else: open_list = np.array(open_list) X = list(open_list[np.where((open_list[:, 0] == x0) & (open_list[:, 1] == y0))][0]) open_list = list(open_list.tolist()) # # 把Y放入open_list中 map_grid[x0, y0] = 3 else: if ((close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] != x0 or close_list[ np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] != y0) and (hx > hy + h1)) and \ map_grid[x1, y1] == 4 and hy > k_old: if map_grid[x1, y1] != 3: Y = list(close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1))][0]) close_list = list(close_list.tolist()) close_list.remove(Y) open_list.append(Y) else: open_list = np.array(open_list) Y = list(open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1))][0]) open_list = list(open_list.tolist()) # 把Y放入open_list中 map_grid[x1, y1] = 3 return [open_list, close_list, map_grid] # 扩散程序 process-state:先弹出openlist列表中k最小的节点,并删除这个节点。然后分类处理: def process_state(open_list, close_list, map_grid): # 修改这个点的h值 open_list.sort(key=lambda x: x[3]) # 先排序,选择k最小的节点 X = open_list[0] # X表示k最小的节点 x0 = int(X[0]) y0 = int(X[1]) close_list.append(X) # 将它放入closelist map_grid[x0, y0] = 4 open_list.remove(X) # 从openlist中删除这个节点 # 分类处理:(该节点处于lower状态,该节点处于lower状态) k_old = X[3] hx = X[2] # print(close_list) if k_old < hx: # k_old是上升状态 x1 = x0 y1 = y0 - 1 h1 = 10 [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list) x1 = x0 - 1 y1 = y0 - 1 h1 = 14 [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list) x1 = x0 - 1 y1 = y0 h1 = 10 [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list) x1 = x0 - 1 y1 = y0 + 1 h1 = 14 [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list) x1 = x0 y1 = y0 + 1 h1 = 10 [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list) x1 = x0 + 1 y1 = y0 + 1 h1 = 14 [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list) x1 = x0 + 1 y1 = y0 h1 = 10 [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list) x1 = x0 + 1 y1 = y0 - 1 h1 = 14 [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list) # 找它的邻节点,看能不能让它的h降低 #print(hx) # if k_old == hx: # 该节点x处于lower状态 # x1 = x0 # y1 = y0 - 1 # h1 = 10 # [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # # x1 = x0 - 1 # y1 = y0 - 1 # h1 = 14 # [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # # x1 = x0 - 1 # y1 = y0 # h1 = 10 # [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # # x1 = x0 - 1 # y1 = y0 + 1 # h1 = 14 # [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # # x1 = x0 # y1 = y0 + 1 # h1 = 10 # [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # # x1 = x0 + 1 # y1 = y0 + 1 # h1 = 14 # [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # # x1 = x0 + 1 # y1 = y0 # h1 = 10 # [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # # x1 = x0 + 1 # y1 = y0 - 1 # h1 = 14 # [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # # else: # x1 = x0 # y1 = y0 - 1 # h1 = 10 # [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # # x1 = x0 - 1 # y1 = y0 - 1 # h1 = 14 # [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # # # x1 = x0 - 1 # y1 = y0 # h1 = 10 # [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # # # x1 = x0 - 1 # y1 = y0 + 1 # h1 = 14 # [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # # # x1 = x0 # y1 = y0 + 1 # h1 = 10 # [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # x1 = x0 + 1 # y1 = y0 + 1 # h1 = 14 # [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # # x1 = x0 + 1 # y1 = y0 # h1 = 10 # [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) # x1 = x0 + 1 # y1 = y0 - 1 # h1 = 14 # [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, # map_grid) open_list.sort(key=lambda x: x[3]) # 先排序,选择k最小的节点 k_min = open_list[0][3] # return [open_list, list(close_list), map_grid,k_min,hx] while k_min<hx: [open_list, close_list, map_grid,k_min,hx] = process_state(open_list, close_list, map_grid) #避障 second_path = [] close_list = np.array(close_list) xn = 0 yn = 3 while xn != 7 or yn != 3: list1 = list(close_list[np.where((close_list[:, 0] == xn) & (close_list[:, 1] == yn))][0]) xn = int(list1[4]) yn = int(list1[5]) second_path.append(list1) second_path.append([7, 3, 0, 0, None, None]) draw_effect(map_grid,second_path) print("Find it")

完成第二次搜索图。
D* 算法融合了D算法和A* 算法,可以处理局部动态障碍,运算速度很快。
原本的伪代码是这样的,我根据我地图的实际情况进行了改变。

k_old<h(x): 当前h(x)升高说明原来的路径已经不是最优的了,如果在x周围能找到一个点,h.y+c(x,y)更小,那就修改x的父节点,重置其h的值。
k_old=h(x): 它的父节点是X,但是h.y却不等, 设想一下说明这说明h.y被更改了,但是父节点还没有变。
参考原文链接:D*规划算法及python实现
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。