D*算法(Dynamic A Star)

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值

 接下去是对当前点x进行邻域分析,将两种类型的点进行添加处理:
 1、 Y是X的子节点,并且其h值受到了影响
 2、 Y不是X的子节点,但其h值可以因x变得更小

L8: if K_old=h(x) then----- 在L7中,更新h值后,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值发生了改变
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(x)>h(Y)+c(x,y) and----- Y不是X的子节点,并且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*算法

  • 4
    点赞
  • 20
    收藏
  • 打赏
    打赏
  • 0
    评论

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

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

打赏作者

qq_43133135

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

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

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

打赏作者

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

抵扣说明:

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

余额充值