D*算法的思考与理解

 D*算法：

特点：后向搜索，或者说是反向计算。按照论文中的结果，比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点的最小代价。

 

设想第一次搜索完成后：

每个节点的h和k是相等的。搜索的停止条件是把s点从open表中弹出，或者说s点状态是‘closed’，这与dijkstr算法是相同的。此时寻找到了这样的一条路径。可以看出，每个节点的指向（即节点的父节点）都是周围八个节点中k最小的那个。我们期望的路径出现了，并且每个节点到G的最短路径也标识出来了（+号标识），这样，当出现动态变化时，我们可以利用计算的这个图尽快修改我们的路径，而不是replan。

注意，此时openlist表不为空，里面的节点是比我们s的k值还大的节点，也就是标记为墙的节点（#），当然还有以s为源点扩展的节点，因为扩展后s才被请出openlist。

D*将处理这样变化的情况：我们规划的路径上的点，出现了拥堵。如果不是规划的路径上的点的变化，不予处理，因为我们按原计划仍能到达。

当我们检测到某点被阻碍了，我们可以迅速定位该节点的子节点，因为是反向指针，其实就是定位到了我们被堵的路径点的上一个点，那么我们要做如下事:

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被更改了，但是父节点还没有变。

————————————————————分割线————————————————————

再看D*算法的总结与思考：

D*算法常用于移动机器人领域的路径规划，其衍生算法如Focused D* ，D* lite，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），只需要把（2,3）的父节点从（4,3）改为（4,1），就能够降低其h的值，它的最终h,k将变为6.2+1.4=7.6 就变为lower态，这一改变也将扩散到（2,1）（2,2）点。

最终，只需要把（2,3）的父节点从（4,3）改为（4,1）即可，因为后面到目标点的路径其实是之前计算过的，不必计算。这样就保证了对动态障碍物再规划的效率。最终的路径规划为：