最详细的D*算法详解

史上最详细的D算法详解
D
算法是非启发式的算法,讲解D算法的案例来自于网上的一个D*算法图解PPT,该PPT看似讲解清晰,但经过与实际D算法的代码比较,实则存在一定的错误。

一、D*算法中的相关概念及变量定义

在这里插入图片描述
白色结点表示算法还没有探索过的区域,灰色结点表示障碍物O,蓝色带点的结点表示终点G,蓝色结点表示起点S,黄色结点表示机器人沿着最优路径从起点向终点跳转过程中,突然多出来的障碍物结点。在讲解D*算法之前,需要定义一些变量
在这里插入图片描述
X:机器人所在的当前结点,当前机器人在结点(6,6)——该结点x坐标为6,y坐标也为6。
Y:通常为结点X的邻居结点,结点(6,6)的邻居结点为(5,6)、(5,5)、(6,5)、(7,5)、(7,6)。
b(X):b表示从哪个结点跳到当前结点的,如当前机器人在结点X,坐标为(6,6),那结点(6,6)的b(X)=(7,6),表示机器人上一步从结点(7,6)跳转过来。记为(6,6)—>(7,5),—>表示指向,方向同图1中的右下角。依此类推:
机器人从结点(6,6)跳转到(5,6),记为(5,6)—>(6,6);
机器人从结点(6,6)跳转到(5,5),记为(5,5)—>(6,6);
机器人从结点(7,6)跳转到(6,5),记为(6,5)—>(7,6);
机器人从结点(7,6)跳转到(7,5),记为(7,6)—>(7,6);
c(X,Y):表示机器人从结点X跳转到结点Y的代价。在一个九宫格中,设机器人在中心点,则中心点为X,其邻接点均为Y。规定机器人每次只能跳转一步,则机器人有8个方向可以跳转,分别为上下左右4个方向,斜对角4个方向。对于通畅结点向通畅结点的跳转,假设上下左右跳转的代价c(X,Y)=1,斜对角跳转的代价c(X,Y)=1.4。对于通畅结点向障碍物结点的跳转,此时会调用is_collision()函数判断当前结点的下一跳是否为障碍物,如果是则c(X,Y)为无穷大。
c(Y,X):当然,机器人也可以从结点Y向结点X跳转,此时的代价记为c(Y,X)。值与结点X跳转结点Y相同。
t(X):结点X的状态,包含NEW、OPEN、CLOSED三种状态。NEW状态:节点从未访问过,最初的设置,可能存在部分节点永远处于改状态。OPEN状态:后续规划中需要访问的结点,存储在open list队列中,如结点(5,6)、(5,5)、(6,5)、(7,5)均存储在open list中。CLOSED状态:从open list队列中移出的结点或已经被考察过的结点。如结点(6,6)刚从open list移出还未放入closed list,结点(6,6)正在考察中,之后,结点(6,6)就会被移入closed list中。而结点(7,6)已经考察完,因而结点(7,6)已经被移入closed list中。
h(X):用来存储路径代价,指从当前结点X跳转到终点G的路径。
k(X):k(X)=min{hnew(X),hold(X)},当X结点被加入到open list中式,最小的h(X)值。
INSERT node AND h_new TO open list语句:把结点node及其h值插入open list中。在插入open list前需要判断,如果:
a.结点node的tag为NEW,则该结点的k=h_new
b.结点node的tag为OPEN,则该结点的k=min{k(node),h_new}
c. 结点node的tag为CLOSED,则该结点的k=min{h(node),h_new}
然后h(node)的值修改为h_new,结点node的tag修改为OPEN,最后再把结点node连同其k值一并加入到open list中。
LOWER State(降态):对于某个结点X,k(X)=h(X),向其邻居传播有关c(X,Y)降低的信息。kold为open list中所有结点最小k值。
if kold==h(X) // 当结点X为降态时
----for each neighbor Y of X: // 对于其除障碍物外的所有邻居结点Y
----if t(Y)==NEW or (b(Y)==X and h(Y)!=h(X)+c(X,Y)) or (b(Y) !=X and h(Y)>h(X)+c(X,Y)):
--------b(Y)=X
--------INSERT Y AND h(X)+c(X,Y) TO open list
Raise State(升态):对于某个结点X,k(X)<h(X),向其邻居传播关于c(X,Y)增加的信息。kold同样为open list中所有结点最小k值。
if kold<h(X) // 当结点X为升态时
----for each neighbor Y of X: // 对于其除障碍物外的所有邻居结点Y
----if h(Y)≤kold and h(X)>h(Y)+c(Y,X):
--------b(X)=Y // 修改结点X指向Y,即结点X由跳转而来
--------h(X)=h(Y)+c(Y,X)
----if t(Y)==NEW or (b(Y)==X and h(Y) !=h(X)+c(X,Y)):
--------b(Y)=X
--------INSERT Y AND h(X)+c(X,Y) TO open list
----else:
--------if b(Y) !=X and h(Y)>h(X)+c(X,Y):
------------INSERT X AND h(X) TO open list
--------else:
------------if b(Y)!=X and h(X)>h(Y)+c(Y,X) and t(Y) ==CLOSED and h(Y)> kold:
---------------INSERT Y AND h(Y) TO open list
上述所有变量和状态都是为PROCESS-STATE()函数服务的,PROCESS-STATE()函数伪代码如下:
在这里插入图片描述
MODIFY-COST函数:当算法沿着最优路径前进时,发现前方突然出现障碍物,算法将把当前点X到其邻居结点Y的跳转成本c(X,Y)修改为无穷大,详见c(X,Y)的定义。
Modify-Cost (X, Y):
----if t(X) == CLOSED:
--------INSERT X AND h(b(X))+c(X,b(X)) TO open list

二、D*算法的计算流程

1.D*算法开始就是,机器人从终点向起始点执行Dijkstra’s搜索算法。图中设终点G为(7,6),起点S为(2,1)。开始令结点的h(X),k(X)均为0,结点(7,6)连带其k值一并被放入open list中。
在这里插入图片描述
2.把结点(7,6)从open list中移出,开始考察其邻居结点(6,6)、(6,5)、(7,5),如果机器人从(7,6)向(6,6)跳转,记为(6,6)—>(7,6)。如图右下角。此时由于k(X)=h(X)=0,X为LOWER State:
if kold ==h(X) // 当结点X为降态时
----for each neighbor Y of X: // 对于其除障碍物外的所有邻居结点Y
----if t(Y)==NEW or (b(Y)==X and h(Y)!=h(X)+c(X,Y)) or (b(Y) !=X and h(Y)>h(X)+c(X,Y)):
--------b(Y)=X
--------INSERT Y AND h(X)+c(X,Y) TO open list // kold(Y)为空值,视为无穷大
上述伪代码的意思是,机器人在X(7,6)点,考察X结点的所有邻居结点Y((6,6)、(6,5)、(7,5)),如果这些Y结点的tag标签t(Y)标签全部为NEW,那么后面的两个判断不用考虑,直接把这些Y结点的指向b(Y)修改为X;这些Y结点的k(Y)=min{kold(Y),h(X)+c(X,Y)},kold(Y)为空值,视为无穷大,显然k(Y)=h(X)+c(X,Y);h(Y)=h(X)+c(X,Y)。注意这里c(X,Y)左跳,下跳值为1,斜跳值为1.4。修改图中的h(Y)和k(Y)值,Y的结点坐标连同k(Y)一并送入open list队列中,当所有Y结点进入open list后,所有在open list中的结点按照其k值从小到大排序,如果k值相同则按结点逆时针方向排序。完成上述操作后,结点X(7,6)就会被送进closed list队列。
在这里插入图片描述
3.依此类推,结点(6,6)移出队列,即当前X在结点(6,6),考察(6,6)周围结点Y。
在这里插入图片描述
4.不考虑Gate结点,Obstacle结点的h(Y)和k(Y)值均为无穷大,这里赋值10000,实际上是无穷大。注意,这里无穷大+1.4仍然等于无穷大。因此,10000+1.4或者10000+3.4仍然等于10000,而不等于10001.4和10003.4。
在这里插入图片描述
5.到此,Dijkstra’s搜索算法形成一条最优路径。注意:回溯形成最优路径时看右下角箭头。
在这里插入图片描述
6.当机器人从起点S(2,1)向终点G(7,6)进发时,走到结点(3,2),发现下一步跳转的结点是障碍物,如图中黄色结点所示。
在这里插入图片描述

下面机器人沿着D算法原有最优路径行走时,遇到了前方障碍,开始动态规划新的路径(fuking said a half)。
!!!
Add obstacle at: s = 4 , y = 3
结点(2, 1)的前方畅通无阻
结点(3, 2)的前方发现障碍物
调用MODIFY-COST函数
open list中插入结点(3, 2)及其的父节点(4, 3)的h值4.24+代价inf
open list插入前{(1, 2), (1, 1), (1, 3)}
open list插入前的k值[7.66, 8.07, 8.07]
open list插入前的h值[7.66, 8.07, 8.07]
待插入open list的结点(3, 2)的tag为CLOSED
open list插入后{(1, 2), (1, 1), (3, 2), (1, 3)}
open list插入后的k值[7.66, 8.07, 5.66, 8.07]
open list插入后的h值[7.66, 8.07, inf, 8.07]
当前结点为(3, 2)其k值5.66以及h值inf
kold为5.66
kold<h
kold!=h
结点(3, 2)的邻居结点为[(3, 1), (4, 1), (2, 1), (2, 2)],邻居结点(3, 1)符合第一个else的if条件
邻居结点(3, 1)的父节点修改为(3, 2)
open list中INSERT邻居结点(3, 1)AND结点(3, 2)的h值inf+代价1.0
open list插入前{(1, 2), (1, 1), (1, 3)}
open list插入前的k值[7.66, 8.07, 8.07]
open list插入前的h值[7.66, 8.07, 8.07]
待插入open list的结点(3, 1)的tag为CLOSED
open list插入后{(1, 2), (3, 1), (1, 1), (1, 3)}
open list插入后的k值[7.66, 6.66, 8.07, 8.07]
open list插入后的h值[7.66, inf, 8.07, 8.07]
结点(3, 2)的邻居结点为[(3, 1), (4, 1), (2, 1), (2, 2)],邻居结点(4, 1)符合第三个else的if条件
open list中INSERT邻居结点(4, 1)AND邻居结点(4, 1)的h值6.24
open list插入前{(1, 2), (3, 1), (1, 1), (1, 3)}
open list插入前的k值[7.66, 6.66, 8.07, 8.07]
open list插入前的h值[7.66, inf, 8.07, 8.07]
待插入open list的结点(4, 1)的tag为CLOSED
open list插入后{(1, 2), (4, 1), (3, 1), (1, 1), (1, 3)}
open list插入后的k值[7.66, 6.24, 6.66, 8.07, 8.07]
open list插入后的h值[7.66, 6.24, inf, 8.07, 8.07]
结点(3, 2)的邻居结点为[(3, 1), (4, 1), (2, 1), (2, 2)],邻居结点(2, 1)符合第一个else的if条件
邻居结点(2, 1)的父节点修改为(3, 2)
open list中INSERT邻居结点(2, 1)AND结点(3, 2)的h值inf+代价1.41
open list插入前{(1, 2), (4, 1), (3, 1), (1, 1), (1, 3)}
open list插入前的k值[7.66, 6.24, 6.66, 8.07, 8.07]
open list插入前的h值[7.66, 6.24, inf, 8.07, 8.07]
待插入open list的结点(2, 1)的tag为CLOSED
open list插入后{(1, 2), (2, 1), (4, 1), (3, 1), (1, 1), (1, 3)}
open list插入后的k值[7.66, 7.07, 6.24, 6.66, 8.07, 8.07]
open list插入后的h值[7.66, inf, 6.24, inf, 8.07, 8.07]
结点(3, 2)的邻居结点为[(3, 1), (4, 1), (2, 1), (2, 2)],邻居结点(2, 2)符合第一个else的if条件
邻居结点(2, 2)的父节点修改为(3, 2)
open list中INSERT邻居结点(2, 2)AND结点(3, 2)的h值inf+代价1.0
open list插入前{(1, 2), (2, 1), (4, 1), (3, 1), (1, 1), (1, 3)}
open list插入前的k值[7.66, 7.07, 6.24, 6.66, 8.07, 8.07]
open list插入前的h值[7.66, inf, 6.24, inf, 8.07, 8.07]
待插入open list的结点(2, 2)的tag为CLOSED
open list插入后{(1, 2), (2, 2), (2, 1), (4, 1), (3, 1), (1, 1), (1, 3)}
open list插入后的k值[7.66, 6.66, 7.07, 6.24, 6.66, 8.07, 8.07]
open list插入后的h值[7.66, inf, inf, 6.24, inf, 8.07, 8.07]
当前结点为(4, 1)其k值6.24以及h值6.24
kold为6.24
kold=h
结点(4, 1)的邻居结点为[(3, 1), (3, 2), (5, 1), (5, 2)],邻居结点(3, 1)符合kold=h的if条件
邻居结点(3, 1)的父节点修改为(4, 1)
open list中INSERT邻居结点(3, 1)AND结点(4, 1)的h值6.24+代价1.0
open list插入前{(1, 2), (2, 2), (2, 1), (3, 1), (1, 1), (1, 3)}
open list插入前的k值[7.66, 6.66, 7.07, 6.66, 8.07, 8.07]
open list插入前的h值[7.66, inf, inf, inf, 8.07, 8.07]
待插入open list的结点(3, 1)的tag为OPEN
open list插入后{(1, 2), (2, 2), (2, 1), (3, 1), (1, 1), (1, 3)}
open list插入后的k值[7.66, 6.66, 7.07, 6.66, 8.07, 8.07]
open list插入后的h值[7.66, inf, inf, 7.24, 8.07, 8.07]
结点(4, 1)的邻居结点为[(3, 1), (3, 2), (5, 1), (5, 2)],邻居结点(3, 2)符合kold=h的if条件
邻居结点(3, 2)的父节点修改为(4, 1)
open list中INSERT邻居结点(3, 2)AND结点(4, 1)的h值6.24+代价1.41
open list插入前{(1, 2), (2, 2), (2, 1), (3, 1), (1, 1), (1, 3)}
open list插入前的k值[7.66, 6.66, 7.07, 6.66, 8.07, 8.07]
open list插入前的h值[7.66, inf, inf, 7.24, 8.07, 8.07]
待插入open list的结点(3, 2)的tag为CLOSED
open list插入后{(1, 2), (2, 2), (2, 1), (3, 1), (1, 1), (3, 2), (1, 3)}
open list插入后的k值[7.66, 6.66, 7.07, 6.66, 8.07, 7.66, 8.07]
open list插入后的h值[7.66, inf, inf, 7.24, 8.07, 7.66, 8.07]
当前结点为(2, 2)其k值6.66以及h值inf
kold为6.66
kold<h
kold!=h
结点(2, 2)的邻居结点为[(1, 2), (2, 1), (3, 1), (1, 1), (3, 2), (1, 3)],邻居结点(1, 2)符合第一个else的if条件
邻居结点(1, 2)的父节点修改为(2, 2)
open list中INSERT邻居结点(1, 2)AND结点(2, 2)的h值inf+代价1.0
open list插入前{(1, 2), (2, 1), (3, 1), (1, 1), (3, 2), (1, 3)}
open list插入前的k值[7.66, 7.07, 6.66, 8.07, 7.66, 8.07]
open list插入前的h值[7.66, inf, 7.24, 8.07, 7.66, 8.07]
待插入open list的结点(1, 2)的tag为OPEN
open list插入后{(1, 2), (2, 1), (3, 1), (1, 1), (3, 2), (1, 3)}
open list插入后的k值[7.66, 7.07, 6.66, 8.07, 7.66, 8.07]
open list插入后的h值[inf, inf, 7.24, 8.07, 7.66, 8.07]
结点(2, 2)的邻居结点为[(1, 2), (2, 1), (3, 1), (1, 1), (3, 2), (1, 3)],邻居结点(1, 1)符合第一个else的if条件
邻居结点(1, 1)的父节点修改为(2, 2)
open list中INSERT邻居结点(1, 1)AND结点(2, 2)的h值inf+代价1.41
open list插入前{(1, 2), (2, 1), (3, 1), (1, 1), (3, 2), (1, 3)}
open list插入前的k值[7.66, 7.07, 6.66, 8.07, 7.66, 8.07]
open list插入前的h值[inf, inf, 7.24, 8.07, 7.66, 8.07]
待插入open list的结点(1, 1)的tag为OPEN
open list插入后{(1, 2), (2, 1), (3, 1), (1, 1), (3, 2), (1, 3)}
open list插入后的k值[7.66, 7.07, 6.66, 8.07, 7.66, 8.07]
open list插入后的h值[inf, inf, 7.24, inf, 7.66, 8.07]
结点(2, 2)的邻居结点为[(1, 2), (2, 1), (3, 1), (1, 1), (3, 2), (1, 3)],邻居结点(1, 3)符合第一个else的if条件
邻居结点(1, 3)的父节点修改为(2, 2)
open list中INSERT邻居结点(1, 3)AND结点(2, 2)的h值inf+代价1.41
open list插入前{(1, 2), (2, 1), (3, 1), (1, 1), (3, 2), (1, 3)}
open list插入前的k值[7.66, 7.07, 6.66, 8.07, 7.66, 8.07]
open list插入前的h值[inf, inf, 7.24, inf, 7.66, 8.07]
待插入open list的结点(1, 3)的tag为OPEN
open list插入后{(1, 2), (2, 1), (3, 1), (1, 1), (3, 2), (1, 3)}
open list插入后的k值[7.66, 7.07, 6.66, 8.07, 7.66, 8.07]
open list插入后的h值[inf, inf, 7.24, inf, 7.66, inf]
当前结点为(3, 1)其k值6.66以及h值7.24
kold为6.66
kold<h
kold!=h
结点(3, 1)的邻居结点为[(3, 2), (4, 1), (2, 1), (2, 2)],邻居结点(2, 1)符合第二个else的if条件
open list中INSERT结点(3, 1)AND结点(3, 1)的h值7.24
open list插入前{(1, 2), (2, 1), (1, 1), (3, 2), (1, 3)}
open list插入前的k值[7.66, 7.07, 8.07, 7.66, 8.07]
open list插入前的h值[inf, inf, inf, 7.66, inf]
待插入open list的结点(3, 1)的tag为CLOSED
open list插入后{(1, 2), (2, 1), (3, 1), (1, 1), (3, 2), (1, 3)}
open list插入后的k值[7.66, 7.07, 7.24, 8.07, 7.66, 8.07]
open list插入后的h值[inf, inf, 7.24, inf, 7.66, inf]
结点(3, 1)的邻居结点为[(3, 2), (4, 1), (2, 1), (2, 2)],邻居结点(2, 2)符合第二个else的if条件
open list中INSERT结点(3, 1)AND结点(3, 1)的h值7.24
open list插入前{(1, 2), (2, 1), (3, 1), (1, 1), (3, 2), (1, 3)}
open list插入前的k值[7.66, 7.07, 7.24, 8.07, 7.66, 8.07]
open list插入前的h值[inf, inf, 7.24, inf, 7.66, inf]
待插入open list的结点(3, 1)的tag为OPEN
open list插入后{(1, 2), (2, 1), (3, 1), (1, 1), (3, 2), (1, 3)}
open list插入后的k值[7.66, 7.07, 7.24, 8.07, 7.66, 8.07]
open list插入后的h值[inf, inf, 7.24, inf, 7.66, inf]
当前结点为(2, 1)其k值7.07以及h值inf
kold为7.07
kold<h
kold!=h
当前结点为(3, 1)其k值7.24以及h值7.24
kold为7.24
kold=h
结点(3, 1)的邻居结点为[(3, 2), (4, 1), (2, 1), (2, 2)],邻居结点(2, 1)符合kold=h的if条件
邻居结点(2, 1)的父节点修改为(3, 1)
open list中INSERT邻居结点(2, 1)AND结点(3, 1)的h值7.24+代价1.0
open list插入前{(1, 2), (1, 1), (3, 2), (1, 3)}
open list插入前的k值[7.66, 8.07, 7.66, 8.07]
open list插入前的h值[inf, inf, 7.66, inf]
待插入open list的结点(2, 1)的tag为CLOSED
open list插入后{(1, 2), (2, 1), (1, 1), (3, 2), (1, 3)}
open list插入后的k值[7.66, 8.24, 8.07, 7.66, 8.07]
open list插入后的h值[inf, 8.24, inf, 7.66, inf]
结点(3, 1)的邻居结点为[(3, 2), (4, 1), (2, 1), (2, 2)],邻居结点(2, 2)符合kold=h的if条件
邻居结点(2, 2)的父节点修改为(3, 1)
open list中INSERT邻居结点(2, 2)AND结点(3, 1)的h值7.24+代价1.41
open list插入前{(1, 2), (2, 1), (1, 1), (3, 2), (1, 3)}
open list插入前的k值[7.66, 8.24, 8.07, 7.66, 8.07]
open list插入前的h值[inf, 8.24, inf, 7.66, inf]
待插入open list的结点(2, 2)的tag为CLOSED
open list插入后{(1, 2), (2, 2), (2, 1), (1, 1), (3, 2), (1, 3)}
open list插入后的k值[7.66, 8.66, 8.24, 8.07, 7.66, 8.07]
open list插入后的h值[inf, 8.66, 8.24, inf, 7.66, inf]
7.k_min为7.66小于等于结点(3, 2)的h值7.66,此时表明D算法已经找到另一条最优路径。下面就是最优路径的回溯了。
结点(3, 2)的前方畅通无阻
结点(4, 1)的前方畅通无阻
结点(5, 2)的前方畅通无阻
结点(6, 3)的前方畅通无阻
结点(7, 4)的前方畅通无阻
结点(7, 5)的前方畅通无阻
到达终点G(7,6),至此D
算法的路径重规划结束。
在这里插入图片描述

  • 23
    点赞
  • 29
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

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

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值