D*算法(Dynamic A Star)

符号及函数说明

Openlist是一个可以用来做广度优先搜索的队列

节点state的标识tag分为三类：没有加入过open表的（new）、在open表的（open）、曾经在open表但现在已经被移走的（closed）。

每个节点到终点G的代价为h，两个节点（state）之间的开销为C（X,Y），X到终点的代价h为上一个节点（父节点）Y到终点的代价+X和Y之间的开销

每个节点的最小h值为k，代表了该点在全图环境中到G点的最小代价，在计算和更新过程中，对于标识为new的点，k=h，对于标识为open的点，k=min{ k，newh}，对于closed的点，k=min{ k，newh}。

算法最主要的是两个函数，Process-State 和 Modify-Cost，前者用于计算到目标G的最优路径，后者用于改变两个节点（state）之间的开销C（X,Y） 并将受影响的节点（state）置于Openlist中。

首次搜索

将终点置于Openlist中，采用Dijkstra进行搜索，直到搜索到了起点，结束。搜索结束后，每个搜索过的节点标识为closed，每个节点的k=h，其父节点为邻域中k值最小的那个。

当搜索结束后，可以通过从起点开始找父节点，一路搜索到终点。若搜索结束时Openlist不为空，则里头的节点h的值必然不必起点的h值小。

碰到障碍

若当前点的下一个点（父节点）为障碍，修改其h值并将其放入Openlist中（如果是墙的话就修改为墙的h值，比如无穷大），但其k值仍旧不变，即k=k=min{ k，newh}，所以该点会被优先取出并且扩散展开。

扩散过程需要利用到论文中的伪代码 process-state，直到k_min>= state_h 。也就是如果扩散到某个节点的时候，计算后的h值不必其上次的k值小，该点就可以结束对外扩散了。

伪代码及注释

Function ： Process-State()   ----------  节点处理函数
L1：X=Min-State（）----- 取所有节点中k值最小的
L2：if X=Null then return -1----- Openlist为空，结束
L3：K_old=Get-Kmin();Delete(X)----- 获得该点k值，并将该点弹出Openlist队列

L4：if K_old<h(x) then----- 比较该点h值与K值，（h值上升状态）
L5: ——  for each neighbor Y of X:----- 遍历邻域节点Y
L6: ———— if h(Y)<= K_old and h(x)>h(y)+c(x,y)----- Y的不处于上升状态，且用其更新x后，h的h值更小
L7: —————— b(x)= Y; h(x)=h(y)+c(x,y)---- 更新x的父节点及其h值

L8: if K_old=h(x) then----- 比较该点h值与K值，（h值未变化状态）
L9: ——  for each neighbor Y of X:----- 遍历邻域节点Y
L10: ———— if t(Y)= New or----- Y是首次添加到Openlist并处理
L11: —————（b(Y)= X and h(Y)!=h(x)+c(x,y)）or----- Y是X的子节点，并且其h值不需要改变
L12: —————（b(Y)！= X and h(Y)>h(x)+c(x,y)）or----- Y不是X的子节点，并且其h值可以变得更小
L13: —————— b(Y)= X; insert（Y，h(x)+c(x,y)）----- 将X作为Y的父节点，修改其h值，并将Y点添加到Openlist中

L14：else ----- 比较该点h值与K值，（h值下降状态）
L15: ——  for each neighbor Y of X:----- 遍历邻域节点Y
L16: ———— if t(Y)= New or----- Y是首次添加到Openlist并处理
L17: —————（b(Y)= X and h(Y)!=h(x)+c(x,y)）or----- Y是X的子节点，并且其h值不需要改变
L18: —————— b(Y)= X; insert（Y，h(x)+c(x,y)）----- 将X作为Y的父节点，修改其h值，并将Y点添加到Openlist中
L19: ———— else
L20: ————— if((b(Y)！= X and h(Y)>h(x)+c(x,y)）then----- Y不是X的子节点，并且其h值可以变得更小
L21: —————— insert（X，h(x)）----- 修改X的h值，并将其点添加到Openlist中
L22: ————— else
L23: —————— if((b(Y)！= X and h(Y)>h(x)+c(x,y) and----- Y不是X的子节点，并且其h值可以变得更小
L24: ——————— t(Y)= Closed and h(Y)>K_old then ----Y不在Openlist中，且h值处于上升状态
L25: ——————— insert（Y，h(Y)）----修改Y的h值，并将其点添加到Openlist中
L25: return Get-Kmin() ----返回该点k值

Function ： Modify-cost(x,y,cval)   ----------  两节点间距离处理函数
L1：c(X,X)=cval ----- 重置两节点间距离
L2：if t(X)=Closed then insert(X,h(x))----- X不在Openlist中，则修改其h值，并添加到Openlist
L3：return Get-Kmin() ----返回X点k值

程序理解

Process-State（）： 用于计算到目标G的最优路径。
从open表中获取k值最小的节点，并移除该点。
对该点进行分类处理，遍历其邻域，看是否需要修改父节点、h值及添加到open表中，分类大体如下：
首先进行一次k_old<h(x)判断,看x的h值是否需要调整：

k_old<h(x): 说明该节点受到障碍的影响，x处于raise状态，可以设想x突变为墙时h值上升，或者其父节点受到突变为墙的节点影响，导致h值升高，最后影响到了他。
然后遍历其邻域，如果y点的h值没有上升，并且x的h值能通过该点变得更小。
上述情况，那就修改x的父节点为y，重置其h的值。

然后再重新判断，看y的h值是否需要调整：

k_old=h(x): 处于第一遍遍历的阶段，或者该节点x并没有受到障碍影响。
然后遍历其邻域，if后面的判断代表：y第一次被遍历到；或者y的父节点是X，但是h(y)和h(x)+c(x,y)值却不等, 由于k_old=h(x)，这说明h(y)被更改了，但是h(x)还没有变；又或者y的父节点不是X，但是如果让X成为其父节点将拥有更小的h(y)值。
上述三种情况都应该根据x的h值调整y的h值，并将x作为y的父节点，并将y添加到open表中

k_old！=h(x): 说明该节点受到影响，遍历其邻域。
如果y为第一次遍历到的点；或者x是y的父节点，但是h(y)和h(x)+c(x,y)值却不等, 这说明h(x)被更改了，但是h(y)还没有变；
上述情况应该应该根据x的h值调整y的h值，并将x作为y的父节点，并将y添加到open表中。

如果y的父节点不是X，但是让X成为其父节点将拥有更小的h(y)值。
上述情况应该应该调整x的h值，并将x添加到open表中。

如果y的父节点不是X，但是让Y成为X父节点，X将拥有更小的h(x)值，并且y被已经被open表移除，且h(y)值在上升(即y受到影响)。
上述情况应该应该调整y的h值，并将y添加到open表中。

小结

调整x的h值及其父节点的情况有：

1、k_old<h(x) &&h(y)<= K_old and h(x)>h(y)+c(x,y)

不调整x的h值，但将x添加到open表中情况有：

1、k_old！=h(x) &&((b(Y)！= X and h(Y)>h(x)+c(x,y)）

调整y的h值及其父节点，并将y添加到open表中情况有：

1、k_old=h(x) && t(Y)= "new"
2、k_old=h(x) &&(b(Y)= X and h(Y)!=h(x)+c(x,y))
3、k_old=h(x) &&((b(Y)！= X and h(Y)>h(x)+c(x,y)）
4、k_old！=h(x) &&t(Y)= New
5、k_old！=h(x) &&(b(Y)= X and h(Y)!=h(x)+c(x,y))

不调整y的h值，但将y添加到open表中情况有：

1、k_old！=h(x) &&(b(Y)!= X and h(Y)>h(x)+c(x,y) && t(Y)= "closed" and h(Y)>K_old

py代码

insert(x,h)

def insert(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 == "closed":
			state.k = min(state.k, h_new)
		state.h = h_new
		state.t = "open"
		open_list.add(state)

min_state()

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

get_kmin()

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

cos(x,y)

   def cost(X, Y):
		if X.state == "#" or Y.state == "#":
			return maxsize
		return X.cost(Y)

process_state()

def process_state():
        x = min_state()
        if x is None:
            return -1
        old_k = get_kmin()
        delete(x)

        if old_k < x.h:
            for y in map.get_neighbors(x):
                if y.h<=old_k and x.h > y.h + cost(x,y):
                    x.parent = y
                    x.h = y.h + x.cost(y)

        if 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 + cost(x,y) or\
                y.parent != x and y.h > x.h + cost(x,y)) )and\
                y != end:
                    y.parent = x
                    insert(y, x.h + cost(x,y))
        else:
            for y in self.map.get_neighbors(x):
                if y.t == "new" or \
                y.parent == x and y.h != x.h + cost(x,y):
                    y.parent = x
                    insert(y, x.h + cost(x,y))
                else:
                    if y.parent != x and y.h > x.h + cost(x,y):
                        insert(x, x.h)
                    else:
                        if y.parent != x and x.h > y.h + cost(x,y) and \
                        y.t == "closed" and y.h > old_k:
                        	insert(y, y.h)

        return get_kmin()

modify_cost(X, Y,cval)

    def modify_cost(X, Y,cval):
    	X.cost(Y)=cval
        if X.t == "closed":
            insert(X, X.h)

D*算法

def D_star(start, end):
        open_list.add(end)
        /'反向Dijkstra'/
        while True:
            process_state()
            if start.t == "close":
                break

        /'寻找路径'/
        s = start
        while s != end:
            s = s.parent
       
        tmp = start # 起始点不变
        map.set_obstacle([(9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8)]) # 设置障碍物
        
        /'添加到openlist并进行重新规划'/
        while tmp != end:
            if tmp.parent.state == "#":
                modify_cost(tmp)
                while True:
		            k_min = process_state()
		            if k_min >= tmp.h:
		                break
                continue
            tmp = tmp.parent
            

参考资料：D*算法