D*规划算法及python实现

mhrobot

于 2020-05-13 21:30:33 发布

7100

分类专栏：规划算法

本文链接：https://blog.csdn.net/qq_36458461/article/details/106106254

版权

规划算法专栏收录该内容

10 篇文章 10 订阅

订阅专栏

1、简介

“D*算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种启发式的路径搜索算法，适合面对周围环境未知或者周围环境存在动态变化的场景。

2、算法介绍

同A算法类似，D-star通过一个维护一个优先队列（OpenList）来对场景中的路径节点进行搜索，所不同的是，D不是由起始点开始搜索，而是以目标点为起始，通过将目标点置于Openlist中来开始搜索，直到机器人当前位置节点由队列中出队为止（当然如果中间某节点状态有动态改变，需要重新寻路，所以才是一个动态寻路算法）。

特点：后向搜索，或者说是反向计算。按照论文中的结果，比replan的效率高不少，在动态环境中表现优秀。这是因为它提前把地图信息都计算并存储的缘故。

里面的节点被称为state,每个state有如下的值：
tag:new,open,closed
分别标识三类节点，分别标识没有加入过open表的、在open表的、曾经在open表但现在已经被移走的。一开始所有的点的tag初值为new，当被加入到open表之后，被置为‘open’ ，从open表移走，被置为‘closed’

h: 每个点的h值代表当前该点，到终点，也就是目标点G的代价。第一次搜索到起点时时，所有点的h会被更新，计算方式同dijkstr算法，是用相邻两点的代价+上一个点的代价累加得到。

k: 该点最小的h值。k的计算在更新过本节点的h后，如果本点‘new’,k=h；如果本点’open’，那就取一下当前k和new_h最小值，如果本点是’closed’，取当前h和new_h最小值； 总而言之，k将会保持到最小，它表示了本点在全图环境中到G点的最小代价。

2.1 设想第一次搜索完成后：

每个节点的h和k是相等的。搜索的停止条件是把s点从open表中弹出，或者说s点状态是‘closed’，这与dijkstr算法是相同的。此时寻找到了这样的一条路径。可以看出，每个节点的指向（即节点的父节点）都是周围八个节点中k最小的那个。我们期望的路径出现了，并且每个节点到G的最短路径也标识出来了（+号标识），这样，当出现动态变化时，我们可以利用计算的这个图尽快修改我们的路径，而不是replan。

注意，此时openlist表不为空，里面的节点是比我们s的k值还大的节点，也就是标记为墙的节点（#），当然还有以s为源点扩展的节点，因为扩展后s才被请出openlist。
D*将处理这样变化的情况：我们规划的路径上的点，出现了拥堵。如果不是规划的路径上的点的变化，不予处理，因为我们按原计划仍能到达。

2.2 当我们检测到某点被阻碍了

我们可以迅速定位该节点的子节点，因为是反向指针，其实就是定位到了我们被堵的路径点的上一个点，那么我们要做如下事:

1.修改该点的h值，即state.h会变的很大，因为他的父节点是墙了，把它放入到open表中。注意此时该节点的k还是小值，是原来的那个h的值，因此，它将被立即取出展开。

2.把这个修改扩散出去，这个扩散过程需要利用到论文中的伪代码 process-state，直到k_min>= state.h 。

注意process-state运行过程中，state.h是会变的，因为该节点的父节点会被改成其他的h小的节点。

核心程序：

在这里插入图片描述

核心程序理解：

弹出open表中最小的节点，并删除这个节点。然后分类处理之：

k_old<h(x): 说明该节点x处于raise状态，可以设想x是之前那个父节点突变为墙的子节点。当前h(x)升高说明原来的路径已经不是最优的了，如果在x周围能找到一个点，h.y+c（x,y）更小，那就修改x的父节点，重置其h的值。

k_old=h(x): 该节点x处于lower的状态，并没有受到障碍影响，或者还在第一遍遍历的阶段。if后面的判断代表：周围是标记为new的节点，代表第一次被遍历到。or 它的父节点是X，但是h.y却不等, 设想一下说明这说明h.y被更改了，但是父节点还没有变。

3. D*算法的总结与思考：

D算法常用于移动机器人领域的路径规划，其衍生算法如Focused D ，D* lite(源于LPA 算法的演变)，Field Ｄ* 目前已经是机器人路径规划的核心算法。
D是动态A（D-Star,Dynamic A Star），卡内及梅隆机器人中心的Stentz在1994和1995年两篇文章提出，主要用于机器人探路。

A适用于在静态路网（环境）中寻路，在环境变化后，往往需要replan，由于A不能有效利用上次计算的信息，故计算效率较低。

D*由于储存了空间中每个点到终点的最短路径信息，故在重规划时效率大大提升。

A是正向搜索，而D特点是反向搜索，即从目标点开始搜索过程。在初次遍历时候，与Dijkstr算法一致，它将每个节点的信息都保存下来。

下图是PPT截图，起点为（2,1），目标终点为(7,6)，搜索中每个节点都有一个反向指针，指向让其h最小的父节点。当起点在open表被弹出时，

从起点到终点的路径就出现了。

h代表该节点到终点，也就是目标点G的代价。k是该节点所有过的h的最小值。b是父节点。
在这里插入图片描述假设我们检测到某点被阻碍了，如上图的（4,3）点，它变成了墙，它的h会被改为一个大值，并将该点和其邻接点重新加入open表中进行搜索，该点 h>k变成了“raise”态，（h=k为lower态）通过搜索，如果它自己周围找不到让h低下来（变为lower）的途径，则它会将这一状态传播到周围。（3,2）点会受影响，它的h将改变，（4,4）（5,4）（5,3）均不会受影响。具体原因可详细看论文。(3,2)点呢，它可以找到点（4,1），只需要把（3,2）的父节点从（4,3）改为（4,1），就能够降低其h的值，它的最终h,k将变为6.2+1.4=7.6 就变为lower态，这一改变也将扩散到（2,1）（2,2）点。
在这里插入图片描述
最终，只需要把（3,2）的父节点从（4,3）改为（4,1）即可，因为后面到目标点的路径其实是之前计算过的，不必计算。这样就保证了对动态障碍物再规划的效率。最终的路径规划为：

4. python代码实现

#!/usr/bin python
#coding:utf-8
import math
from sys import maxsize

class State(object):
	def __init__(self, x, y):
		self.x = x
		self.y = y
		self.parent = None
		self.state = "."
		self.t = "new"
		self.h = 0
		self.k = 0

	def cost(self, state):
		if self.state == "#" or state.state == "#":
			return maxsize
		return math.sqrt(pow(self.x - state.x, 2) + pow(self.y - state.y, 2))

	def set_state(self, state):
		if state not in [".","@", "#", "+", "S", "E"]:
			return
		self.state = state

class Map(object):
	def __init__(self, row, col):
		self.row = row
		self.col = col
		self.map = self.init_map()

	def init_map(self):
		map_list = []
		for i in range(self.row):
			temp = []
			for j in range(self.col):
				temp.append(State(i, j))
			map_list.append(temp)
		return map_list

	def print_map(self):
		for i in range(self.row):
			str_temp = ""
			for j in range(self.col):
				str_temp += self.map[i][j].state + " "
			print(str_temp)

	def get_neighbors(self, state):
		state_list = []
		for i in [-1, 0, 1]:
			for j in [-1, 0, 1]:
				if i == 0 and j == 0:
					continue
				if state.x + i < 0 or state.x + i >= self.row:
					continue
				if state.y + j < 0 or state.y + j >= self.col:
					continue
				state_list.append(self.map[state.x + i][state.y + j])
		return state_list

	def set_obstacle(self, point_list):
		for i, j in point_list:
			if i < 0 or i >= self.row or j < 0 or j >= self.col:
				continue
			self.map[i][j].set_state("#")

class DStar(object):
	def __init__(self, maps):
		self.map = maps
		self.open_list = set()

	def process_state(self):
		#print("In process_state")
		x = self.min_state()
		if x is None:
			return -1
		
		old_k = self.min_k_value()
		self.remove(x)
		

		if old_k < x.h:
			for y in self.map.get_neighbors(x):
				if old_k >= y.h and x.h > y.h + x.cost(y):
					x.parent = y
					x.h = y.h + x.cost(y)
				
		elif old_k == x.h:
			for y in self.map.get_neighbors(x):
				if (y.t == "new" or y.parent == x and y.h != x.h + x.cost(y) or y.parent != x and y.h > x.h + x.cost(y)) and y != end:
					y.parent = x
					self.insert_node(y, x.h + x.cost(y))
		else:
			for y in self.map.get_neighbors(x):
				if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y):
					y.parent = x
					self.insert_node(y, x.h + x.cost(y))
				else:
					if y.parent != x and y.h > x.h + x.cost(y):
						self.insert_node(x, x.h)
					else:
						if y.parent != x and x.h > y.h + x.cost(y) and y.t == "close" and y.h > old_k:
							self.insert_node(y, y.h)

		return self.min_k_value()

	def min_state(self):
		if not self.open_list:
			print("Open_list is NULL")
			return None
		return min(self.open_list, key = lambda x: x.k)   # 获取openlist中k值最小对应的节点

	def min_k_value(self):
		if not self.open_list:
			return -1
		return min([x.k for x in self.open_list])     # 获取openlist表中值最小的k


	def insert_node(self, state, h_new):
		if state.t == "new":
			state.k = h_new
		elif state.t == "open":
			state.k = min(state.k, h_new)
		elif state.t == "close":
			state.k = min(state.k, h_new)
		state.h = h_new
		state.t = "open"
		self.open_list.add(state)


	def remove(self, state):
		if state.t == "open":
			state.t = "close"

		self.open_list.remove(state)

	def modify_cost(self, state):
		if state.t == "close":
			self.insert_node(state, state.parent.h + state.cost(state.parent))

	def modify(self, state):  #当障碍物发生变化时，从目标点往起始点回推，更新由于障碍物发生变化而引起的路径代价的变化
		self.modify_cost(state)
		while True:
			k_min = self.process_state()
			if state.h <= k_min:
				break

	def run(self, start, end):
		#self.open_list.add(end)
		self.insert_node(end, 0)
		while True:
			self.process_state()
			if start.t == "close":
				break
		start.set_state("S")
		s = start
		while s != end:
			s = s.parent
			s.set_state("+")
		s.set_state("E")

		print("Before obstacle change!")
		self.map.print_map()

		temp_s = start
		
		self.map.set_obstacle([(12, 4), (12, 5), (12, 6), (12, 7), (12, 8),(12, 9), (12, 10), (12, 11)]) # 障碍物发生变化

		while temp_s != end:
			temp_s.set_state("@")
			if temp_s.parent.state == "#":
				self.modify(temp_s)
				continue

			temp_s = temp_s.parent

		temp_s.set_state("E")

		print("After obstacle change!")
		self.map.print_map()


if __name__ == "__main__":
	mh = Map(25, 25)
	mh.set_obstacle([(6, 3), (6, 4), (6, 5), (6, 6), (6, 7),(7, 4), (7, 5), (7, 6)])

	start = mh.map[3][2]
	end = mh.map[23][16]
	mh.print_map()

	dstar = DStar(mh)
	dstar.run(start, end)

实现效果

在这里插入图片描述
参考文章：
Optimal and Efficient Path Planning for Partially-Known Environments
D路径搜索算法原理解析及Python实现
 D算法的思考与理解

mhrobot

关注

16
点赞
踩
93
收藏
打赏
2
评论
D*规划算法及python实现

1、简介“D*算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种启发式的路径搜索算法，适合面对周围环境未知或者周围环境存在动态变化的场景。2、算法介绍同A算法类似，D-star通过一个维护一个优先队列（OpenList）来对场景中的路径节点进行搜索，所不同的是，D不是由起始点开始搜索，而是以目标点为起始，通过将目标
复制链接

扫一扫