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是动态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",