D-star算法简介及相关思考

   一、D*算法简介

   “D* 算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种增量式启发式的路径搜索算法， 适合面对周围环境未知或者周围环境存在动态变化的场景。

   同 A * 算法类似，D-星算法通过维护一个优先队列（OpenList）来对场景中的路径节点进行搜索，所不同的是，D*不是由起始点开始搜索，而是以目标点为起始，通过将目标点置于Openlist中来开始搜索，直到机器人当前位置节点由队列中出队为止（当然如果中间某节点状态有动态改变，需要重新寻路，所以才是一个动态寻路算法）。

   启发式搜索是利用启发函数来对搜索进行指导，从而实现高效的搜索。增量搜索是对以前的搜索结果信息进行再利用来实现高效搜索，大大减少搜索范围和时间。

   D*算法采用反向搜索的目的在于后期需要重新规划路径的时候，能够用到先前搜索到的最短路径信息，减少搜索量。因为以目标向起始点进行搜索得到的最短路径图，是以目标点为中心辐射出的最短路径图，图上目标点到各点之间都是最短路径，为此其在既定路径上遇到问题需要重新路径规划的时候，可以很好的利用原先得到的信息。而以起始点向目标点搜索得到的最短路径图，其是以起始点为中心辐射出的最短路径图，当沿着既定路径前行遇到障碍物之后，需要重新进行路径规划之时，没有办法很好的利用原先搜索得到的信息。

---

   二、D * 算法总体流程及与A*算法的对比

   从总的来说，D * 算法分为两个阶段，第一个阶段是使用dijkstra / A *算法找到从目标点到起始点的静态可行路径，然后机器人开始从起点向目标点运动。第二个阶段是动态避障搜索阶段，主要用于修正若干个受障碍物影响而导致代价值发生变化的那些节点信息。

   0、初始化：将目标点放入OpenList 中

   与A * 的对比：A*算法初始化时将起始点放到OpenList 中

   1、从OpenList 中找到k值最小的节点，并将该节点从OpenList 中移除，并放到closedlist列表中

   与A * 的对比： A*算法选择拓展点时是从OpenList 列表中选择f值最小的点拓展（f=g+h），而D * 算法是从OpenList 列表中选择k值最小的点拓展

   在D * 算法中是从目标点向起始点进行搜索，我们将从目标点到当前点的代价记为h （其实从原理上来看与这里的h与A * 中 的g更为相似，都是从搜索开始的点到当前点累计的代价值），在D * 中没有使用从当前点到起始点的估计值，也就是没有使用启发信息，从这一点上来看，D * 与dijkstra算法更为相似

   那么D *中的 k值是什么呢？ D * 是针对动态环境设计的算法，由于环境的改变某点处的h值可能发生改变，而某点处的k值其实记录的就是该点的最小h值 ，也就是说，对于还未遍历到的点，k=h=inf;对于标识为open或closed的点，k=min { k , h_new}。

   思考：为什么不选用最小的h值节点来作为拓展点，而是使用k值呢？

   动态环境下，假如某个节点变成了障碍物，此时其h值会被修改为inf，我们需要将这种变化传递下去，若采用h值作为选取标准，该节点会被置于openList中的最后。也就是说，此时路径规划会从openList中剩余的处于OPEN状态的节点开始，一直扩张至全图都没有不可达节点之后，才会访问该节点。这显然并不合理，因为我们的目的就是要在节点状态动态变化的时候减少搜索的空间，提高搜索效率。 而用最小的h值也就是k值在openList中进行排序，表示这里曾有一条捷径，那么就会优先在这附近进行搜索。。

   2、判断当前拓展点的k值是否与该点的h值相同，若k

   我们用x表示该节点，用y表示其某个相邻节点，即若h（y）< k(x),且h（x）> h(y) + c(y ,x),则将当前点x的父节点改为y，并将该点的h值更改为h（x）= h(y) + c(y ,x)，其中c(y ,x)代表从y点到x点的代价值

   我们试图用上面的操作来减少该点处的h值，并期望使其回到Lower态，但是否回到Lower态还需要后续验证

   与A * 的对比： A * 算法中不考虑动态障碍物，从而A*算法中并没有与该步骤对应的操作，在D * 算法中每个节点还有Lower态和Raise态两种状态，若h=k，记为Lower态﹔若h>k,记为Raise态，当该节点处于Raise态时表明有更优的路径。

   3、判断经过第2步的操作后，该点的k值是否与h值相等，即是否回到了Lower态，若相等，则按照以下准则进行节点的拓展：遍历每个相邻节点，若出现以下三种情况之一，则将该相邻节点的父节点设为当前点，并将该相邻节点的h值设为h （x）+ c (x,y)，放到OpenList中。

   ①若该相邻节点从未被拓展过，即该相邻节点还未进OpenList，

   ②若该相邻节点的父节点是当前点，且h（y）≠ h （x）+ c (x,y),

   ③若该相邻节点的父节点不是当前点，且h（y）> h （x）+ c (x,y),

   与A * 的对比：以上三种情况中，情况①和③与 A * 算法的拓展方式相同，较容易理解，而第②种情况，是D *算法考虑动态障碍物后新增的，我们可以这样理解，该相邻节点的父节点是当前点，也就是说正常情况下h（y）= h （x）+ c (x,y)，而此时不等于，说明，环境已经发生了改变，使得h（x）变大了，所以相应的由x点拓展出来的子节点y的h值也要进行更新，并将其重新放到OpenList中，对由y拓展出来的点进行更新，以此更新下去

   3、判断经过第2步的操作后，该点的k值是否与h值相等，即是否回到了Lower态，若不等，即k < h，则按照以下准则进行节点的拓展：

   ①该相邻节点从未被拓展过，即该相邻节点还未进OpenList、②该相邻节点的父节点是当前点，且h（y）≠ h （x）+ c (x,y), 若该相邻节点y是以上两种情况，则将该相邻节点的父节点设为当前点，并将该相邻节点的h值设为h （x）+ c (x,y)，放到OpenList中。

   ③若该相邻节点的父节点不是当前点，且h（y）> h （x）+ c (x,y),则将当前点x加入到OpenList，这里为什么不将y的父节点设定为当前点x，并对其h（y）进行更新呢？，往OpenList里面放到为什么不是y，而是x呢？我们可以这样理解，此时的当前点x处于Raise态，即当前x处的代价值不是最优的，所以，我们将x放到OpenList中，等待下一次循环，当前点的代价值变成最优的后，再对该相邻节点进行处理，以此来保证传递下去的代价值是最小的

   ④若该相邻节点的父节点不是当前点，且h（y）> h （x）+ c (x,y),且该相邻点y在closedlist中，且h（y）> k,则将y重新放到OpenList中，怎么理解呢？ 我们每次从OpenList取点时，取得是k最小的，既然y已经在closedlist中，说明y比当前点先拓展，即k（y）<= k(x)，而现在h（y）> k（x），说明y受到了障碍物的影响，使得h（y）变大了，因此需要将y放到OpenList进行考察

   4、从步骤1开始进行下一轮拓展，依次循环往复，拓展结束后通过路径回溯找到所需的路径

   5、动态阶段：机器人按照规划的路径进行移动的过程中，若当前点为x，且探测到要走的下一个节点y存在障碍，或者节点y本来存在障碍物，但是继续行进到x的时候，障碍物被移除。这时就要调用modify_cost(x,y,val)函数，将最新的cost(x,y)以及h(y)的值进行变更，若此时x已经位于closedlist，则需要将其重新放到openlist中，并反复执行process_state()函数用于传播节点h值变更信息和路径搜索直到process_state()函数返回的openList中所有节点的k值中的最小k值(k_{min}) >= h(x) 或者openList没有任何节点时，表示重新路径规划完成，或者无法找到别的路径从x点规划到目标点G

   思考：为何重新进行路径规划的时候，当执行到 (k_{min}) >= h(y) 时停止？

   因为process_state()函数的功能是用邻居节点来减低自身节点的h值的，当所有处于OPEN状态的节点中的k值的最小值 (k_{min}) 也要大于等于h(y)时，表示不可能再通过process_state()函数的执行来降低h(y)值了，那么自然就没有再搜索的必要，且已经完成了路径的修正。

---

---

   三、D*算法有几个重要的概念及函数：

   G：表示进行路径搜索的目标点

   c(x,y)：从节点x移动到节点y的代价

   t(x)：节点的状态。每个节点(作者论文中称为state)都有一个状态，其中总共有三种可能NEW,OPEN,CLOSED。NEW表示从未加入到openList中的，也就是从未被遍历查找过的。OPEN表示节点在被查找中，节点在openList中。CLOSED表示从openList中被移出。OPEN和CLOSED状态会相互转化，当节点从openList中移出的时候，状态从OPEN变为CLOSED，当节点再次加入openList，进行 降低节点自身h值或者传播当前节点的h值变更信息给邻居节点，让邻居节点进行h值修改变更 操作时，状态从CLOSED变为OPEN

   h(x)：表示地图上的点x到达目标点G的代价。由于D*算法是从目标点开始进行路径规划的，为此，初始化的时候，令h(G) = 0。此后，其代价的变动可能会在两个地方出现，一个是路径搜索的过程中，当节点x的邻居节点被执行搜索过程的时候，如果其能够让h(x)的代价更小，则更新h(x) = h(y) + cost(y,x)。一个是在路径搜索完成后执行路径搜索结果的过程中遇到障碍物之时，通过insert(x,y,val)函数改变障碍物节点y的代价h(y)

   k(x)：可以理解为节点最小的h(x)值。随着节点遍历和搜索过程的进行，节点的h(x)值会不断的变动

   b(x) = y：用于记录当前节点x的父节点，这里b(x) = y表示x节点的父节点为y节点。在搜索完成之后，能够根据b(x)的值，回溯追踪到目标节点

   openList：存放节点状态为OPEN的节点。且其按照节点的k值从小到大进行排序

   process_state()：该函数是用来降低openlist表中的某个节点x(state)的h(x)值或者传播节点x的h(x)值变更信息给邻居节点，让邻居节点进行相应变更的同时进行路径搜索查找的

   insert(x,val)：该函数是用来修改节点x的状态以及h(x)值和k(x)值的

   modify_cost(x,y,val)：该函数是用来修改节点x和y之间的移动代价cost(x,y)，而且根据节点y的状态t(y)的情况，可能对节点y的h(y)值和状态t(y)进行修改

---

   四、D*算法总结：

   相比A-star算法，D-star的主要特点就是由目标位置开始向起始位置进行路径搜索，当物体由起始位置向目标位置运行过程中，发现路径中存在新的障碍时，新的障碍不行影响目标位置到新障碍之间的范围内的路径节点，新障碍只会影响机器人所在位置（当前位置）到障碍之间范围的节点的路径。在这时通过将新的障碍周围的节点加入到Openlist中进行处理然后向物体所在位置进行传播，能最小程度的减少计算开销。 路径搜索的过程中跟Dijkstra算法比较像，并没有体现A-star所具有的方向感，在拓展时没有将当前拓展点与起始点之间的代价估计值作为选取拓展点的考量之一，即没有朝着向起点搜索的引导，这种搜索更多的是一种由目标位置向四周发散搜索，直到把起始位置纳入搜索范围为止。

---

主要参考资料：
     1、D star路径搜索算法：【点击此处跳转】
     2、D * 算法详解 一看就会 手把手推导 完整代码注释：【点击此处跳转】
     3、基础路径规划算法(Dijikstra、A*、D*)总结：【点击此处跳转】
     4、古月学院 基于栅格地图的机器人路径规划算法指南：【点击此处跳转】

---