基于C++的D*算法详解

DStar算法

网上的D*算法大多是用python编的，而且有的还有问题，讲解感觉也不是很透彻，本文解释了核心算法的步骤并以C++为框架搭建了DStar算法，目前在测试用例中未发现问题，如哪位读者发现任何测试用例有问题请及时给予评论，谢谢！

DStar算法即Dynamic A Star，它是一种动态搜索路径的算法，也就是在我们预先找到Start 到 Target之间的路径后，如果在Start通过后向指针向Target运动的过程中，当前点X发现下一个点Y为障碍点，则此时通过以往保存过的路径信息，相比于replan尽可能快的重新找到X到Target的新的路径！

 

 

 

在论文中有用的字母定义

1.每一个Node分别有三个State，即New：表示还未加入到openlist，Open：表示在openlist中，Closed：从openlist中移除

2.Dstar算法是后向搜素，也就是从Target点到Start点利用Dijkstra算法保存最短路径信息，因此H表示当前点到Target（G）点的距离

3.b（x）表示x的后向指针

4.K表示该点所有状态下最小的H值！！！（核心）

5.X表示在openlist找到的k值最小点

6.当K=H表示当前为lower状态(也就是没有障碍堵塞),反之K<H则进去raise状态，需要绕路！

算法核心

1. 引入了k，而且相比于Dijkstra算法是每次找openlist中H最小的点，在DStar算法中是找k最小的点，这是为什么呢？见2

2. 当我们知道了上文Y是一个障碍点，那么我们最想做的就是在X周围找到一条不受X和Y影响的，为lower的路径！因此我们就需要在openlist找到X点，只有这样我们才能在X周围尽可能找到能使X点变为lower的点，而又因为Y作为障碍点，X和Y之间的cost变为无穷大，因此如果不引入K我们只有在最后才能在openlist找到X点，从而大幅降低了效率！

3. 我们需要先把障碍点放进openlist！因为需要把障碍点的raise状态扩散给所有指向障碍点的点

4. 把一个状态为New的X周围的Y节点(new_H)插入openlist中，Y点的k为new_H，把一个状态为open的Y插入openlist其实也就是指的是可以从当前的最优点优化原来到Y的距离，因此只需要对应修改Y的H和k即可，此时k=min(k_old,new_H)，当把一个状态为closed的Y插入openlist中时，此时有两种可能：一种可能就是Y是lower的，这时候就是我们把raise状态的Y点放了进去然后在周围又找到一个次优点，用来更新这个raise点的位置！！对应于L23-L25 ，另一种可能就是Y是raise状态，其实也就对应了上一种可能的下一次process-state的调用，即使得我们的raise点Y的状态变为lower，找到了一条次优的路径，对应于L13，对是L13我没写错，因此k=min(old_H,new_H)，因此这时候raise点就变为lower了

详解算法流程

先说一下如果当前点位(3,2)时碰到的下一个点(4,3)为障碍点时，以该图为例应该怎么做，即沿着蓝线走到(3,2)发现(4,3)变为障碍的流程。

 

 

具体的整个PPT图片可以详见链接

https://zhuanlan.zhihu.com/p/366462473

1. （4，3）的H变为很大的一个数，程序中为cost_max，这时就要把（4，3）加入到openlist中，此时Y的状态变为raise，接下来判断Y的周围的点有没有next指针指向（4，3），也就是执行L15-L18，其余均不执行，如果有那么Y的周围这些next指针指向Y的点的H也变为cost_max，k不变，即被扩散为raise状态，该图中即（3，2）点变为raise加入openlist中其余（4，3）的周围点不加入进去

2. 下一步找到了（3，2）点并弹出，先执行L4-L7，看看周围能不能找到一个k值小的最优点绕出去，该例中没有，随后重复1的过程执行L15-L18即扩散raise状态给周围点，该图为（2，2）、（2，1）、（3，1）点的H变为cost_max࿰