【规划】D* Lite路径搜索算法

原文来来自于 云水禅心_心一

上一篇介绍了D*路径搜索算法原理解析及Python实现，这一篇紧接着介绍D*算法的改进版。

1.D* Lite算法简述

D_star Lite算法是Koenig S和Likhachev M基于LPA_star 算法基础上提出的路径规划算法。

D_star Lite 算法之于 LPA_star 算法犹如 D_star 算法之于 A_star 算法。与 LPA_star 采用的正向搜索算法不同，D_star Lite 采用反向搜索方式，效果与D_star 算法相当。无论是前文提到LPA_star 算法还是A_star 算法都不能满足移动机器人在未知环境中的路径规划需求，因为其在未知地图中需要不断的尝试，与边走边找到最优路径背道而驰。此时反向搜索算法能够很好的处理这种情况，D_star 算法虽然可以实现未知环境的路径规划，但效率较低，基于 LPA_star 的D_star Lite可以很好的应对环境未知的情况，其算法核心在于假设了未知区域都是自由空间，以此为基础，增量式地实现路径规划，通过最小化rhs值找到目标点到各个节点的最短距离。在移动机器人按照规划的路径进行前进时其所到的节点即设置为起始节点，因此路径变化或者key值需要更新时，需要更新从目标点到新起点的启发值以及估计成本。由于移动机器人不断的靠近目标点，节点的启发值将不断减少，且减少至不会超过h（start Org,start New）。由于每次都要减去相同的值，开启列表的顺序并不会改变，因此可以不进行这部分的计算，这样便避免了每次路径改变时的队列遍历过程。

若前行过程中发现障碍物则将障碍物所对应环境地图位置设置为障碍物空间，并再以之为起点利用“路径场”信息重新规划出一条路径来。此时不仅更新规划路径的节点数据，也要更新智能体遍历过的节点。其关键点在于如何在未知的环境中根据传感器获取的极少周边地图信息来进行最有效的靠近目标点的任务。其实整个靠近的过程一直在扩大已知地图范围，尽可能少的尝试次数来实现完成抵达目标点的任务。下图为 D_star Lite 搜索示意图，黑点是在按照反向搜索的路径执行时发现的障碍点，到遇到不能通行的障碍点后便更新地图信息，重新规划出一条新的路径继续前行。

D_star Lite 算法的原理类似 D_star ，起初需要根据已知的环境信息，未知部分视作自由空间，规划出从目标点到起始点的全局最优路径，此时即建立了一个“路径场”信息，为增量靠近目标点提供择优依据。D_star Lite算法是反向搜索的，因此LPA_star 里的g(s)，h(s)有了新的定义，即分别代表从目标点到当前s点的代价值以及当前s点到出发点的启发值。与LPA_star 中相反，g*(s) 记录栅格节点的前继节点，计算式为

rhs(s)记录栅格节点的后继节点的g(s)，有公式：

在评价栅格点的估价值时 D_star Lite 也引入了 k(s) 值进行比较，其中 k(s) 包含两个值[k(s1); k(s2)] ，分别满足以下公式：

 与 LPA_star算法相对应，可以很容易得出以下公式：

2.D* Lite算法伪代码

D* Lite 路径规划算法描述如下表:

3.D*Lite算法一个简单的例子

3.1 地图无变化时

以一个具体例子来讲解算法的工作流程：

首先计算出由起始点B1→目标点E3的启发值h如上图。h的值近似于从起始网格到当前网格的这两个网格的x和y坐标的绝对差异的最大值。D*Lite算法具体的过程如下图。

在初始化Initialization阶段，将所有可行的网格中的g=rhs=∞，即执行算法2-6步。其中k=[k_1;k_2]按照公式计算即可。

    在第一步step1中，即要计算E3周围的点，D2、E2和D3。以D3为例计算，先按公式计算rhs(s)=min(g(s’)+c(s,s’))=1，再计算k[k_1;k_2]=[3;1]。由此可以计算D2、E2点如图。比较key值，选择最小的一个，即为D2。D2的g值按照公式min_s(c(s’,s)+g(s’)=0+1=1更新为1，再继续计算。
    在第二步step2中，需要以D2为中心，对C1，C2，C3，D1，D2，D3，E1，E2，E3都进行计算。按照第一步step1的计算方法，各值计算如图。其中D3的key为[3;1]最小（最优），故选D3为进一步扩展点。
    在第三步step3中，需要以D3为中心，对C2，C3，D2，D3，E2，E3都进行计算。各值计算如图，其中C1的key为[3;2]最小（最优），故选C1为进一步扩展点。
    在第四步step4中，需要以C1为中心，对B1，B2，C2，D1，D2都进行计算。各值计算如图，其中B1的key为[3;3]最小（最优），故选B1为进一步扩展点。
    在第五步step5中，需要以B1为中心，对A1，A2，B2，C1，C2都进行计算。各值计算如图，A1为起点，算法结束。

因此路径为A1→B1→C1→D2→E3。

3.2地图变化时

当地图中的一个点D2变为障碍物时，起始点变为B1，位置为C1，目标点仍为E3，启发值h变为如下图：

此时，k_m=k_m+h(s_last,s_start)=0+1=1,更改各点的启发值h如上图（算法28-31行）。D*Lite算法更新如下图：

首先是计算变化的cost，即Edge Cost Changes步。改变的为C1。其根据计算式rhs(s)=min_s Succ(s)(c(s’,s)+g(s’)=1+3=4。此时g(s)小于rhs(s)为欠一致性，即遇到障碍物，g(u)=∞，更新所有的前继节点（算法19-20行）。因此从step1开始按照之前的路径重新搜索，但此时不需要再计算太多其他的网格。所以，回到E2开始搜索（step1），进而在D1处搜索（step2），再到C1处搜索，进而达到当前的机器人位置，终止搜索。
因此更新的路径为C1→D1→E2→E3。

4.算法总结

D_star Lite结合了D_star 算法动态规划的特性（由目标位置开始向起始位置进行路径搜索。当路径中存在新的障碍时，对于目标位置到新障碍之间的范围内的路径节点，新的障碍是不会影响到其到目标的路径的）和LPA_star 算法的利用增量式搜索特性。

D_star Lite算法能够很好的适用于未知环境做路经规划，由于其增量规划的思想，它可以做到较少重规划次数以及较少的重规划影响节点数。但是当状态空间比较大，也就是环境地图比较大的时候，采用的 D_star Lite 路径规划算法的反向搜索过程需要维护的栅格节点数急剧增加，增加了搜索的时间复杂度。除此之外，D_star Lite 路径规划不能应对环境复杂的情况，即局部环境的精细规划。对于大环境下的路径规划，D_star Lite 算法的做法是将环境地图进行更细粒度的栅格化，虽然在足够细粒化的环境地图中可以实现较优的路径解，但也会带来更多的规划序列导致执行次数以及重规划次数增多，进而路径规划执行花费时间也会变得更长。

参考资料

[1 ]路径规划——D* Lite算法
[2] LPA和DLite算法
[3] Koenig S, Likhachev M. Fast replanning for navigation in unknown terrain[J]. IEEE Transactions on Robotics, 2005, 21(3): 354-363.
[4] 徐开放. 基于D*Lite算法的移动机器人路径规划研究[D]. 哈尔滨工业大学, 2017.

[5] 路径规划——D* Lite算法