[PathPlanning]D*

概述

本文详细介绍了D*算法原理，它是一种动态搜索路径的算法。D*算法主要解决位置地形条件下路径重新规划(replan)问题，当移动机器人碰到障碍时候通过局部修正快速得到最优的路径规划。相比于重新利用Dijkstra或者A*做为Replanner，D*对运行时间的优化随着随着搜索搜索空间增长有着显著的优化。

重要定义

• $G$定义为目标节点

• $b(X)=Y$：表示$Y$为$X$的$backpointer$，即$X$为$Y$的父节点，因此$G$不存在父亲节点

• $tag(X)$表示节点$X$的状态，目前区分$New,Open,Closed$

  • $tag(X)=New$：节点$X$不在$openlist$中
  • $tag(X)=Open$：节点$X$在当前$openlist$中
  • $tag(X)=Closed$：节点$X$不再在$openlist$中

• $H(X)$表示节点$X$到目标节点$G$的启发式函数

• $c(X,Y)$表示从$Y$到$X$边的权重，同时定义$X$和$Y$是邻居节点

• $k(X)$定义为$X$在$openlist$中最小的$h(X)$数值

  • 如果$k(X)<h(X)$，定义为$X$为$Raise$，D*使用$openlist$中$Raise$状态传播路径损失增加的信息

  • 如果$k(X)=h(X)$，定义为$X$为$Lower$，D*使用$openlist$中$Lower$状态传播路径损失减少的信息

• $k_{min}$为$openlist$中所有节点$k(X)$的最小值，即$\min (k(X))$，最优路径损失小于等于$k_{min}$

• $k_{old}$定义为上一轮$k_{min}$的数值

算法框架

1. 将目标节点加入到$openlist$，$h(G)=0$
2. 循环执行progress_state函数直到满足下面三个条件之一
   1. $k_{min}=-1$
   2. 初始节点从$openlist$中删除(与A*算法逻辑类似)
   3. $openlist$为空 (与Dijkstra算法逻辑类似)
3. 如果$k_{min}=-1$，表示目标节点不可到达，结束程序
4. 循环追踪最优路径
   1. 如果到达目标节点，结束程序
   2. 如果地图环境发生变化(遇到障碍物$Y$)
      1. 调用modify_cost(Y,X,c(X,Y))函数修改边的cost，并将节点$Y$加入$openlist$
      2. 循环执行progress_state函数搜索绕开障碍物$Y$的路径，直到$k_{min}\ge h(X)\&\&k_{min}!=-1$，其中$X$为当前状态节点。(这个终止条件表明$openlist$中最小的$k$值已经比$h(X)$值大了，不能再找到更小的$k_{min}去优化路径$。)
      3. 如果$k_{min}=-1$，表示目标节点不可到达，结束程序

核心算法解释

progress_state

progress_state主要功能是计算到目标节点的最优cost。该算法可以简单分为四个大模块。

模块1

该模块主要是从$openlist$选择最小$k$值得节点，保存最小$k_{min}$到$k_{old}$，最后调用delete(X)函数从$openlist$中删除$X$节点。这里delete(X)函数核心功能是$tag(X)=Closed$。

模块2

该模块判断节点$k_{old}<h(X)$是否是$Raise$状态，表明该节点已经受到障碍物得影响，那么搜索能够降低$h(X)$邻居节点。如果找到满足条件，那么把$Y$设置为$X$的父亲节点，同时更新$h(X)$的值。该过程只能够减小$h(X)$，但是与$h_{old}$的关系尚未可知，需要下面的模块进一步判断。

模块3

该模块是判断了$X$节点为$Lower$状态，然后继续判断邻居节点$Y$是否有必要$X$作为父亲节点。如果$X$和邻居节点$Y$满足下面三个条件之一，需要把$Y$节点加入到$openlist$，进一步考察

• 情况1：$Y$节点为$New$，表示还未加入到$openlist$；
• 情况2：$X$为$Y$的父亲节点，但是不满足$h(Y)\ne h(X)+c(X,Y)$，说明$h(x)$更新过，可能是由于障碍物引起的
• 情况3：$X$不是$Y$的父亲节点，但是$h(Y)>h(X)+c(X,Y)$，说明$Y$可以利用$X$减小$h(Y)$

模块4

主要处理$X$为$Raise$情况下扩展，该情况下受到障碍物的影响了。可以分为以下三个模块：

模块4.1

当满足下面条件之一，将$Y$节点加入到$openlist$

• 情况1：$Y$节点状态为$New$，表明还未被探索
• 情况2：$X$为$Y$节点的父亲节点，但是$h(Y)\ne h(X)+c(X,Y)$，表明$X$节点受到障碍无影响

模块4.2

如果$X$不是$Y$节点的父亲节点，但是$h(Y)>h(X)+c(X,Y)$， 表明$Y$节点可以通过$X$减少$h(Y)$，但是因为$X$是$Raise$状态，需要$X$加入到$openlist$中待下一次满足条件，将$X$作为$Y$节点的父亲节点(对应于模块3中的条件)。

模块4.3

如果$X$不是$Y$节点的父亲节点，且$h(X)>h(Y)+c(X,Y)$，且$tag(Y)=Closed$，且$h(Y)>k_{old}$，表明$Y$节点能够通过$X$节点减小$h(Y)$，但是$h(Y)$居然比最小的$k_{old}$故而需要重新把$Y$大，故而需要加入到$openlist$中

modify_cost(X, Y, c)

modify_cost函数主要是用修改边的cost函数，并将受影响的状态加入$openlist$

1. 更新所有的$c(X,Y)$
2. 如果$tag(X)=Closed$，将$X$加入到$openlist$
3. 返回$openlist$中的$k_{min}$

insert

insert函数主要的功能是将需要考虑的节点加入到$openlist$，并更新状态。

1. 如果$tag(X)=New$，$k(X)=h_{new}$
2. 如果$tag(X)=Open$，$k(X)=\min (k(X),h_{new})$
3. 如果$tag(X)=Closed$，$k(X)=\min (h(X),h_{new})$
4. $h(X)=h_{new}$
5. $tag(X)=Open$

例子

如上图所示，D*算法静态搜索能够得到从起点$(2,1)$到$(7,6)$的最短路径，其中用蓝色标记。在该过程中，从目标节点反向搜索过程中主要调用progress_state中模块3，此时根据问题求解方式可以等价于Dijkstra或者A*算法。假设移动机器人运动到$(3,2)$发现$(4,3)$是障碍物，此时需要执行replan的流程，这一部分也是D*的核心。

1. Replan过程首先把$(4,3)$加入到$openlist$中，然后对邻居节点进行扩展。此时$h(4,3)$是一个极大的值，该节点状态也变成了$Raise$状态。因为与$(4,3)$连接的其他邻居连接都是最大cost，所以模块2不被执行，此时只有模块4.1被执行了，也就是将$(3,2)$加入到$openlist$中，$h(3,2)$设置为最大值
2. 接下来会将$(3,2)$弹出作为当前节点进行搜索，此时状态也是$Raise$状态。此时$k_{old}=5.6$，模块2中搜索周围发现不存在邻居节点的$h(Y)\le k_{old}$，因此不执行。通过模块4.1，将$Raise$状态扩散给$(2,2),(2,1),(3,1)$，并将$h$值设置为最大值。通过模块4.3将$(4,1)$加入到$openlist$。此时，$openlist=\{(2,2),(2,1),(3,1),(4,1)\}$
3. 下一步弹出$(4,1)$，$k_{old}=6.2$，该节点为$Lower$状态，因此只需要执行模块3。对周围邻居进行探索，$(3,2)$满足情况3，将$(3,2)$设置为$(4,1)$的子节点，同时使$(3,2)$变成$Lower$状态加入到$openlist$中。此时$k(3,2)=7.6$。
4. 其他的节点按照类似的关系一直更新，直到所有的返回的$k_{min}\ge h(3,2)$，说明$openlist$中不存在节点能够优化$h(3,2)$

Reference

1. D*
2. Robotic Motion Planning: A* and D* Search
3. Optimal and Efficient Path Planning for Partially-Known Environments