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

本文深入解析DStar(动态A*)算法,重点在于C++实现,适用于动态路径搜索。介绍了算法核心概念,如后向搜索、关键路径K值,并通过实例解释算法流程。此外,提供了C++代码实现,使用优先级队列优化查找效率。
摘要由CSDN通过智能技术生成

DStar算法

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

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

watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBA5LqI5bex5Y-K5Lq6,size_17,color_FFFFFF,t_70,g_se,x_16 

 f1d97edc55d84bbe94dbe3ae90a32759.png

 

在论文中有用的字母定义

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)变为障碍的流程。

 watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBA5LqI5bex5Y-K5Lq6,size_15,color_FFFFFF,t_70,g_se,x_16

 

具体的整个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࿰

评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值