D*路径搜索算法
“D_star算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种启发式的路径搜索算法，适合面对周围环境未知或者周围环境存在动态变化的场景。
同A_star算法类似，D-star通过一个维护一个优先队列（OpenList）来对场景中的路径节点进行搜索，所不同的是，D*不是由起始点开始搜索，而是以目标点为起始，通过将目标点置于Openlist中来开始搜索，直到机器人当前位置节点由队列中出队为止（当然如果中间某节点状态有动态改变，需要重新寻路，所以才是一个动态寻路算法）。
符号表示
主要介绍一下论文中用到的一些符号及其含义。
论文中将地图中的路径点用State表示，每一个State包含如下信息：
- Backpointer: 指向前一个state的指针，指向的state为当前状态的父辈，当前state称为指针指向state的后代，目标state无Backpointer。（路径搜索完毕后，通过机器人所在的state，通过backpointer即可一步步地移动到目标Goal state，GoalState以后用 G表示），b（X）=Y表示X的父辈为Y。
- Tag：表示当前state的状态，有 New、Open、Closed三种状态，New表示该State从未被置于Openlist中，Open表示该State正位于OpenList中，Closed表示已不再位于Openlist中。
- H（X）：代价函数估计，表示当前State到G的开销估计。
- K（X）：Key Function，该值是优先队列Openlist中的排序依据，K值最小的State位于队列头，对于处于OpenList上的State X，K（X）表示从X被置于Openlist后，X到G的最小代价H（X），可以简单理解为K（X）将位于Openlist的State X划分为两种不同的状态，一种状态为Raise（如果K（X）<H（X）），用来传递路径开销的增加（例如某两点之间开销的增加，会导致与之相关的节点到目标的路径开销随之增加）；另一种状态为 Lower（如果K（X）=H（X）），用来传递路径开销的减少（例如某两点之间开销的减少，或者某一新的节点被加入到Openlist中，可能导致与之相关的节点到目标的路径开销随之减少）。
- kmin：表示所有位于Openlist上的state的最小K值。
- C（X,Y）：表示X与Y之间的路径开销。
- Openlist 是依据K值由小到大进行排序的优先队列。
算法描述
搜索的关键是state的传递过程，即由G向机器人所在位置进行搜索的过程，这种传递过程是通过不断地从当前OpenList中取出K值最小的State来实现的，每当一个State由Openlist中移出时，它会将开销传递给它的邻域state，这些邻域state会被置于Openlist中，持续进行该循环，直到机器人所在State的状态为 Closed ，或者Openlist为空（表示不存在到G的路径）。
算法最主要的是两个函数，Process-State 和 Modify-Cost，前者用于计算到目标G的最优路径，后者用于改变两个state之间的开销C（X,Y）并将受影响的state置于Openlist中。
算法的主要流程，在初始时，所有state的t（Tag）被设置为New，H（G）被设置为0，G被放置于OpenList，然后Process-State函数被不断执行，直到机器人所处state X由openlist中出队，然后可以通过机器人的当前state按backpointer指向目标G。当移动过程中发现新探测到的障碍时，Modify-Cost函数立刻被调用，来更正C（X,Y）中的路径开销并将受影响的state重新置于openlist中。
令Y表示robot发现障碍时所在的state，通过不断调用Process-State直到kmin≥H（Y），这时表示路径开销的更改已经传播到了Y，此时，新的路径构建完成。
论文中的伪代码如下：
简要解释为：
- L1-L3表示拥有最低K值的X由openlist中移出，如果X为Lower，那么它的路径代价为最优的。
- 在L8-L13，X为Lower状态，X的所有邻接state都被检测是否其路径代价可以更低，状态为New的邻接state被赋予初始路径开销值，并且开销的变动被传播给每一个backpointer指向X的邻接state Y（不管这个新的开销比原开销大或者小），也就是说只要你指向了X，那么X的路径开销变动时，你的路径代价必须随之改变。这里可能存在由于X路径开销变动过大，Y可以通过非X的其他state到达目标且路径开销更小的情况，这点在L8-13中并没有处理，而是放在后续针对Y的process-state函数中，在对Y进行处理时，会将其backpointer指向周围路径开销最小的state。如果X的邻接State状态为New时，应将其邻接state的backpointer指向X。所有路径开销有所变动的state都被置于Openlist中进行处理，从而将变动传播给邻接的state。
- 在L4-L7中X为Raise，它的路径开销H可能不是最优的，通过其邻居state中已经处于最优开销（即h（Y）≤kold）的节点来优化X的路径开销，如果存在更短的路径，则将X的backpointer指向其neighbor。
- 在L15-L18中，开销变动传播到状态为New的邻居state。如果X可以使一个backpointer并不指向X的邻居state的路径开销最小，即Y通过X到目标G的距离更短，但是此时Y的backpointer并不指向X，针对这种情况，可以将X重新置于Openlist中进而优化Y。
- 在L23-25中，如果X可以通过一个状态为closed的并不是最理想的邻居stateY来减小路径开销，那么将Y重新置于Openlist中。
在modify-cost中，更新C（X,Y）并将X重新置于Openlist中，当X通过process-state进行传播时，会对Y进行开销计算，h(Y)=h(X)+c(X,Y)。
D*算法的另一种理解
一般的表述通常有两种方式，一种用永久和临时标号方式，一种是用OPEN, CLOSE表方式，这里均采用OPEN,CLOSE表的方式。
chinaliping 的 文章 最短路经算法简介(Dijkstra算法，A*算法，D*算法) 中对D*有个简单的描述：
- 先用Dijstra算法从目标节点G向起始节点搜索。储存路网中目标点到各个节点的最短路和该位置到目标点的实际值h,k（k为所有变化h之中最小的值,当前为k=h。原OPEN和CLOSE中节点信息保存。
- 机器人沿最短路开始移动，在移动的下一节点没有变化时，无需计算，利用上一步Dijstra计算出的最短路信息从出发点向后追述即可，当在Y点探测到下一节点X状态发生改变，如堵塞。机器人首先调整自己在当前位置Y到目标点G的实际值h(Y)，h(Y)=X到Y的新权值c(X,Y)+X的原实际值h(X).X为下一节点(到目标点方向Y->X->G），Y是当前点。k值取h值变化前后的最小。机器人沿最短路开始移动，在移动的下一节点没有变化时，无需计算，利用上一步Dijstra计算出的最短路信息从出发点向后追述即可，当在Y点探测到下一节点X状态发生改变，如堵塞。机器人首先调整自己在当前位置Y到目标点G的实际值h(Y)，h(Y)=X到Y的新权值c(X,Y)+X的原实际值h(X).X为下一节点(到目标点方向Y->X->G），Y是当前点。k值取h值变化前后的最小。
- 用A_star或其它算法计算，这里假设用A_star算法,遍历Y的子节点，点放入CLOSE,调整Y的子节点a的h值，h(a)=h(Y)+Y到子节点a的权重C(Y,a),比较a点是否存在于OPEN和CLOSE中，方法如下：用A或其它算法计算，这里假设用A算法,遍历Y的子节点，点放入CLOSE,调整Y的子节点a的h值，h(a)=h(Y)+Y到子节点a的权重C(Y,a),比较a点是否存在于OPEN和CLOSE中，方法如下：
while()
{
从OPEN表中取k值最小的节点Y;
遍历Y的子节点a,计算a的h值 h(a)=h(Y)+Y到子节点a的权重C(Y,a)
{
if(a in OPEN) 比较两个a的h值
if( a的h值小于OPEN表a的h值 )
{
更新OPEN表中a的h值;k值取最小的h值
有未受影响的最短路经存在
break;
}
if(a in CLOSE) 比较两个a的h值 //注意是同一个节点的两个不同路径的估价值
if( a的h值小于CLOSE表的h值 )
{
更新CLOSE表中a的h值; k值取最小的h值;将a节点放入OPEN表
有未受影响的最短路经存在
break;
}
if(a not in both)
将a插入OPEN表中; //还没有排序
}
放Y到CLOSE表；
OPEN表比较k值大小进行排序；
}Python实现
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time : 2018/12/13 0013 22:30
# @Author : 心一
# @Site :
# @File : D_star.py
# @Software: PyCharm
import math
from sys import maxsize # 导入最大数，2^63-1
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 # k即为f
def cost(self, state):
if self.state == "#" or state.state == "#":
return maxsize # 存在障碍物时，距离无穷大
return math.sqrt(math.pow((self.x - state.x), 2) +
math.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
map_list = []
for i in range(self.row):
tmp = []
for j in range(self.col):
tmp.append(State(i, j))
map_list.append(tmp)
return map_list
def print_map(self):
for i in range(self.row):
tmp = ""
for j in range(self.col):
tmp += self.map[i][j].state + " "
print(tmp)
def get_neighbers(self, state):
# 获取8邻域
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 x, y in point_list:
if x < 0 or x >= self.row or y < 0 or y >= self.col:
continue
self.map[x][y].set_state("#")
class Dstar(object):
def __init__(self, maps):
self.map = maps
self.open_list = set() # 创建空集合
def process_state(self):
'''
D*算法的主要过程
:return:
'''
x = self.min_state() # 获取open list列表中最小k的节点
if x is None:
return -1
k_old = self.get_kmin() #获取open list列表中最小k节点的k值
self.remove(x) # 从openlist中移除
print('**************x state',x.x,x.y,x.k,x.h)
# 判断openlist中
if k_old < x.h:
for y in self.map.get_neighbers(x):
if y.h <= k_old and x.h > y.h + x.cost(y):
x.parent = y
x.h = y.h + x.cost(y)
print('k_old < x.h',x.k,x.h)
elif k_old == x.h:
for y in self.map.get_neighbers(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
#if y.x == 17 and y.y == 1
#print('Debug: ',y.x,y.y,y.k,y.h)
self.insert(y, x.h + x.cost(y))
print('k_old == x.h',x.h,x.k)
else:
for y in self.map.get_neighbers(x):
if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y):
y.parent = x
self.insert(y, x.h + x.cost(y))
else:
if y.parent != x and y.h > x.h + x.cost(y):
self.insert(x, x.h)
else:
if y.parent != x and x.h > y.h + x.cost(y) \
and y.t == "close" and y.h > k_old:
self.insert(y, y.h)
print('k_old > x.h',x.h,x.k)
return self.get_kmin()
def min_state(self):
if not self.open_list:
return None
min_state = min(self.open_list, key=lambda x: x.k) # 获取openlist中k值最小对应的节点
return min_state
def get_kmin(self):
# 获取openlist表中k(f)值最小的k
if not self.open_list:
return -1
k_min = min([x.k for x in self.open_list])
return k_min
def insert(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.h, 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, x):
if x.t == "close": # 是以一个openlist，通过parent递推整条路径上的cost
self.insert(x, x.parent.h + x.cost(x.parent))
def run(self, start, end):
self.open_list.add(end)
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('障碍物未发生变化时，搜索的路径如下：')
self.map.print_map()
tmp = start # 起始点不变
self.map.set_obstacle([(9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8)]) # 障碍物发生变化
'''
从起始点开始，往目标点行进，当遇到障碍物时，重新修改代价，再寻找路径
'''
while tmp != end:
tmp.set_state("*")
# self.map.print_map()
# print("")
if tmp.parent.state == "#":
self.modify(tmp)
continue
tmp = tmp.parent
tmp.set_state("E")
print('障碍物发生变化时，搜索的路径如下(*为更新的路径)：')
self.map.print_map()
def modify(self, state):
'''
当障碍物发生变化时，从目标点往起始点回推，更新由于障碍物发生变化而引起的路径代价的变化
:param state:
:return:
'''
self.modify_cost(state)
while True:
k_min = self.process_state()
if k_min >= state.h:
break
if __name__ == '__main__':
m = Map(20, 20)
m.set_obstacle([(4, 3), (4, 4), (4, 5), (4, 6), (5, 3), (6, 3), (7, 3)])
start = m.map[1][2]
end = m.map[17][11]
dstar = Dstar(m)
dstar.run(start, end)
# m.print_map()运行效果如下：
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
