D*规划算法及python实现

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* 目前已经是机器人路径规划的核心算法。
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算法的思考与理解

  • 16
    点赞
  • 93
    收藏
  • 打赏
    打赏
  • 2
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
©️2022 CSDN 皮肤主题:大白 设计师:CSDN官方博客 返回首页
评论 2

打赏作者

mhrobot

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值