提示:前文写了D搜索算法,是一种贪心算法。
文章目录
- 一、D*算法是什么?
- 二、原理以及代码步骤
- 1.原理分析
- 2.代码解释
- 总结
一、D*算法是什么?
D*算法也是用于机器人路径规划问题的启发式方法,它是一种局部规划方法,即仅仅已知一部分地形,对地形的未知部分进行假设,并在这些假设下找到当前坐标到目标坐标的最短路径。然后机器人沿着这条路走,当它观察到新的地图信息(如从前未知的障碍)时,将这些信息添加到地图中,并在必要时重新规划从当前坐标到给定目标坐标的新的最短路径。重复这个过程,直到达到目标坐标或无法达到目标坐标。
二、原理以及代码步骤
1.原理分析
如上图的空间,给定起点(绿色点)、目标点(G蓝色)、障碍(红色),如何进行路径规划?
- 首次搜索
将终点G置于openlist中,采用Dijkstra进行搜索,规定上下左右的代价为10,斜着方向的代价为14。不熟悉Dijkstra算法的朋友看我之前的博文Dijkstra算法在python中的实现。
大概说一下,每个格子包含有三个量(节点名,代价,父节点),从终点A开始,搜索周围的邻节点,给它们分别标号:B,C,D,E,F,代价按照规定的计算(上下左右的代价为10,斜着方向的代价为14),并把终点A放入close list里面(不再遍历)。然后找出代价最小的节点作为新节点,继续搜索,在搜索过程中,如果有节点K已经有代价,但是以新的点为父节点,这个节点K的代价值更小,那么更新其代价值和父节点。
搜索结束条件就是终点从openlist中弹出进入了closelist。
最终,我们搜索到了起点,结束搜索。从起点开始找父节点,一路搜索到终点。 - 遇到障碍
如果机器人在按照原来规划路径行进的时候,路上遇到障碍(不在规划路径上的障碍忽略不计)。例如(3,3)遇到障碍。
修改这个点的h值为无穷大(inf),并且令障碍的所有子节点的h都为无穷大。
图中浅绿色节点是路径上障碍的子节点X,子节点X的h变为inf,将节点X从closelist中弹出,放入openlist列表。
接下来就是将这个改变进行扩散。
先取出openlist中k值最小的节点,还是浅绿色节点X(k=50,h=inf)
找X的周围邻接点Y,如果能够让X的h值变小,就让Y成为X的父节点,这样X(3,2)的父节点变为(4,3),hx变为58,此时X的子节点如(3,1),而hx+10(x和y的代价)不等于hy(60),说明障碍的影响没有扩散到子节点,所以更改子节点(3,1)的h值为hx+10。
因为(4,3)节点到目标点的路径其实是之前计算过的,所以不必计算。
扩散的结束条件就是k_min(openlist中所有节点最小的k值)>=hx(当前点X的h值)。
就能找到一条路径:
2.代码解释
- D算法首次搜索
# coding=utf-8
import matplotlib.pyplot as plt
import numpy as np
import mathmap_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 + h0k = h1 + h0open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 2] = hopen_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 3] = kopen_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 4] = x0open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 4] = y0open_list = list(open_list.tolist())else: # 是new节点h = h1 + h0k = h1 + h0# open_list = list(open_list)open_list.append([x1, y1, h, k, x0, y0])map_grid[x1, y1] = 3return 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 = x0y1 = y0 - 1h1 = 10open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)x1 = x0 - 1y1 = y0 - 1h1 = 14open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)x1 = x0 - 1y1 = y0h1 = 10open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)x1 = x0 - 1y1 = y0 + 1h1 = 14open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)x1 = x0y1 = y0 + 1h1 = 10open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)x1 = x0 + 1y1 = y0 + 1h1 = 14open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)x1 = x0 + 1y1 = y0h1 = 10open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)x1 = x0 + 1y1 = y0 - 1h1 = 14open_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] = 0close_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] = x1close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 5] = y1close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 2] = hy + h1hx = hy + h1return [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] = x0close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] = y0close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2] = hx + h1Y = 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] = 3return [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] = x0close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] = y0close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2] = hx + h1Y = 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] = 3else: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] = 3else: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] = 3return [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) # 将它放入closelistmap_grid[x0, y0] = 4open_list.remove(X)# 从openlist中删除这个节点# 分类处理:(该节点处于lower状态,该节点处于lower状态)k_old = X[3]hx = X[2]# print(close_list)if k_old < hx: # k_old是上升状态x1 = x0y1 = y0 - 1h1 = 10[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)x1 = x0 - 1y1 = y0 - 1h1 = 14[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)x1 = x0 - 1y1 = y0h1 = 10[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)x1 = x0 - 1y1 = y0 + 1h1 = 14[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)x1 = x0y1 = y0 + 1h1 = 10[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)x1 = x0 + 1y1 = y0 + 1h1 = 14[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)x1 = x0 + 1y1 = y0h1 = 10[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)x1 = x0 + 1y1 = y0 - 1h1 = 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实现