D*(Dynamic A*) 算法详细解析

最初是Anthony Stentz发表在ICRA上，《Optimal and Efficient Path Planning for Partially-Known Environments》

记号

1）$G$：终点
2）$X$：表示当前节点，用$b(X)=Y$表示$Y$是$X$的下一个节点，称为后向指针（backpointer），通过后向指针，很容易获得一条从起点到终点的路径。
3）$c(X,Y)$：从$X$到$Y$的cost，对于不存在边的两个节点，$c(X,Y)$可定义为无穷大。
4）同$A^{\ast }$ 一样，$D^{\ast }$ 也用$OPEN$表和$CLOSED$表来保存待访问和已经访问的节点。对每个节点$X$，用$t(X)$来表示$X$的状态，$NEW$表示未添加到$OPEN$，$OPEN$表示当前处于$OPEN$ list，$CLOSED$表示已从$OPEN$ list剔除。
5）同$A^{\ast }$ 一样，$D^{\ast }$ 也有一个对目标的估计函数，$h(G,X)$，不同的是，$D^{\ast }$ 还多了一个key函数，对于在$OPEN$ list中的每个节点，$k(G,X)$的定义原文中为“be equal to the minimum of $h(G,X)$ before modification and all valuses assumed by $h(G,X)$ since $X$ was placed on the $OPEN$ list”。也就是说，h函数会随着拓扑结构的变化而变化，key始终是h中最小的那一个。key函数将$OPEN$中的节点$X$分为两类，一类记为$RAISE$，如果$k(G,X)<h(G,X)$，另一类记为$LOWER$，如果$k(G,X)=h(G,X)$。也就是说，$k(G,X)\le h(G,X)$，小于的情况如前方出现障碍物导致某些边的cost增加，等于的情况如前方cost不变或由于障碍物移动导致某些边cost减小。
6）$k_{min}$和$k_{old}$，前者表示当前$OPEN$中key的最小值，后者表示上一次的最小值。
7）如果一系列节点$X_1,X_2,...,X_N$满足$b(X_{i+1})=X_i$，则称为一个序列（sequence）。一个序列表示了一条从$X_N$到$X_1$的路径。在静态路径规划中，我们通常找的是一条从起点到终点的路径中，但是在动态路径规划中，起点是不断变化的，严格来讲应该是找寻一条从当前节点到终点的路径。显然这种情况下，考虑从终点到当前节点更为方便，故这里$X_1$反而是最靠近终点的节点。称一个序列为单调（monotonic）的如果满足$t(X_i)=CLOSEDandh(G,X_i)<h(G,X_{i+1})$或者$t(X_i)=OPENandk(G,X_i)<h(G,X_{i+1})$。杂乱无序的序列是没有意义的，单调的序列其实就是我们要找的一条合理的路径。
8）对$X_i,X_j$，如果$i<j$则称$X_i$是$X_j$的祖先（ancestor），反之则称为后代（descendant）。
9）如果记号涉及两个节点且其中一个为终点，略去终点，如$f(X)=f(G,X)$

算法描述

$D^{\ast }$主要包括两个部分，$PROCESS-STATE$和$MODIFY-COST$，前者用来计算终点到当前节点的最优cost，后者用来修正cost。主要流程如下：
1）将所有节点的$tag$设置为$NEW$，$h(G)$设为0，将G放置在$OPEN$中。
2）重复调用$PROCESS-STATE$直到当前节点$X$从$OPEN$中移除，即$t(X)=CLOSED$，说明此时已经找到一条从X出发到达终点的路径。
3）根据2）中获得的路径，从节点$X$往后移动，直到达到终点或者检测到cost发送变化。
4）当cost发生变化时，调用第二个函数$MODIFY-COST$，并将因为障碍物而cost受到影响的节点重新放入$OPEN$中。假设$Y$是发现状态时机器人所处的节点。通过调用$PROCESS-STATE$直到$k_{min}\ge h(Y)$，此时cost的变化已经传播到$Y$，因此可以找到一条新的从$Y$到终点的最优路径。
论文中的伪代码如下：

这里$MIN-STATE$和$GET-MIN$的区别是前者返回的是具有最小$k$值的节点，后者返回的是$k_{min}$。而$INSERT(X,h_{new})$则是按照$t(X)$的状态分为三种情况：
a）$t(X)=NEW,k(X)=h_{new}$
b）$t(X)=OPEN,k(X)=min(k(X),h_{new})$
c）$t(X)=CLOSED,k(X)=min(h(X),h_{new}),h(X)=h_{new}andt(X)=OPEN$
最后一个条件就是针对已规划路径cost发生变化的状态。

梳理一下上述流程
1）在静态条件下，利用Dijkstra或A*算法找到一条最优路径，同时利用后向指针确定每个节点的下一节点。
2）机器人从起点出发，沿路径移动，考虑最简单的情况，假设机器人的传感器范围为1，也就是说，只要下一个节点没有障碍物，就移动到下一个节点。
3）假设下一节点出现障碍物，怎么进行$MODIFY-COST$呢？参考伪代码，当某个节点出现障碍物了，可以认为$h_{new}=\infty$，也就是说，这时$k$仍是无障碍时的值，而$h$已经变成无穷了，并且，$X$被重新放入$OPEN$中。
4）放入$OPEN$当然是要重新规划，这里就涉及到一个关键问题了

已经有$h$函数了，为什么还要定义$key$函数？

其实这里的$h$函数应该和$A^{\ast }$中的$f$函数是等价的，虽然只有到终点的开销，但是因为$D^{\ast }$是终点开始的，起点就是当前节点$X$，所以可以认为$h(X)=0,f(X)=g(X)+h(X)=g(X)$。在静态情况下，直接用$h(X)$作为排序，是可以找到一条全局最优路径的（实际就是Dijkstra）。在动态情况下，假设现在下一节点$Y$变成了不可达，那么相应的$h(X)=\infty$，将$X$状态添加进$OPEN$ list，那么$X$节点就是排在$OPEN$ list的最后。也就是说，此时会从$OPEN$ list 剩余处于$OPEN$状态的节点开始，一直到扩张全图都没有不可达节点之后，才会访问$X$，这显然是不合理的。我们的目的就是要减少搜索空间，提高效率。用最小$h$作为$key$来排序，表示这里曾经有一条捷径，那么我优先在这附近搜索，进一步的，当障碍物消失，也能第一时间恢复到最短路径。

那么接下来就是最关键的步骤，怎么通过局部调整避开障碍物找到一条合理的路径（实际是贪心的最优）。

5）此时$PROCESS-STATE$进入的是$L4-L7$，注意，这里我们最简单的想法应该是找到除了Y之外最短的路径，也就是循环迭代$H(X)>H(Y)+C(Y,X)$，但是作者这里加了一个条件，$h(Y)\le k_{old}$，初衷很好理解，我可以绕障碍物，但是不能绕太远，还是要往终点的方向靠。但也有一个新的问题，假设估计采用的是曼哈顿距离，邻域考虑最简单的八邻域，那么如下图所示，红色是当前节点，蓝色是初始规划的下一节点，此时被障碍物占据，满足$h(Y)\le k_{old}$的节点用绿色表示，考虑极端情况，所有绿色格子也被障碍物占据，此时算法会陷入死循环（去掉该条件则不会）。这里暂时不考虑极端情况，沿着作者的思路走（我们可以默认在已知环境中突然出现大量的障碍物概率很小）。

6）在5）中我们找到一个当前最优的下一节点，注意，因为这里是贪心策略，所以并没有考虑到下一节点是否是可行的（可能下一节点的下一节点仍然是被阻塞的）。这样做的好处是尽可能地利用了之前已经探索的区域，减少了搜索空间，坏处就是可能会绕远路。
7）上述过程的终止条件，$k_{min}\ge h(Y)$，也就是说，当前所有处于$OPEN$状态的节点，都不可能再降低cost了，那么自然也就没有继续搜索的必要了。从而就完成了一次路径的修正。
8）这里考虑的是cost增加的情况，实际cost减少的情况也是类似的。

参考

Optimal and Efficient Path Planning for Partially-Known Environments
https://blog.csdn.net/lqzdreamer/article/details/85055569#_437