D* 算法

历史
1994 年 Anthony Stentz 提出了 D* 算法
Anthony Stentz 又于 1995 年提出了 Focused D* 算法
2002年提出了D* Lite算法
2005年 Koening和Likhache: Fast replanning navigation in unknown terrain
A*算法
Life-long planning A*
主要概念
节点状态及扩展
初始化运行
最短路径搜索
伪代码
特点
举例
总结
D*
符号说明
算法描述
算法总结
D* Lite算法
算法介绍
算法总结
D* 描述2
参考

TOC

历史

1994 年 Anthony Stentz 提出了 D* 算法

它将规划的方向进行逆转，从目标点向起始点规划，这样可以处理环境部分未知或全部未知以及动态障碍的一些情况。此算法被作为路径规划的核心算法应用于美国 1996年发射的火星探测器上，并获得了很大的成功。
Stentz A. Optimal and Efficient Path Planning for Partially-known Environments[C]//IEEE International Conference on Robotics and Automation, 1994. Proceedings. 1994:3310 - 3317.

DSTAR 算法由卡耐基梅隆大学机器人学者 Stentz 于 1994 年，在 IEEE机 器 人 与 自 动 化 国 际 会 议 （ IEEE International Conference on Robotics and Automation）上发表[8]。经历了多年的研究和实践验证，DSTAR 已经在机器人应用和各种自主移动车的应用中发挥了重要而广泛的作用，甚至已经被集成在自主火星探测车的软件系统中，作为路径规划的核心算法。

Anthony Stentz 又于 1995 年提出了 Focused D* 算法

重点提高了 D* 算法的实时性
Stentz A. The Focussed D* Algorithm for Real-Time Replanning[C]// International Joint Conference on Artificial Intelligence. 2000:3310-3317.

2002年提出了D* Lite算法

Koenig S, Likhachev M. D*lite[C]// Eighteenth National Conference on Artificial Intelligence and Fourteenth Conference on Innovative Applications of Artificial Intelligence, July 28 - August 1, 2002, Edmonton, Alberta, Canada. 2002:476-483.

Koenig 和 Likhachev 在2002 年提出 DSTAR Lite 是也是一种增量的、启发式的规划算法，它完成的功能和Focussed DSTAR 相似，但从算法设计的角度来看，它们有着不同的思路

算法降低了 D* 路径规划的算法复杂度

2005年 Koening和Likhache: Fast replanning navigation in unknown terrain

Koenig, S. and Likhachev, M. (2005) ‘Fast replanning for navigation in unknown terrain’, IEEE Transactions on Robotics, 21(3), pp. 354–363. doi: 10.1109/TRO.2004.838026.

A*算法

Life-long planning A*

LPA_start或life Planning A_star是一种基于A* 的增量启发式搜索算法。2001年，斯文·柯尼格(Sven Koenig)和马克西姆·利卡切夫(Maxim Likhachev)首次提出。
LPA_star是A_star的增量版本，它可以适应图形中的变化而无需重新计算整个图形，方法是在当前搜索期间更新前一次搜索的g值（从开始起的距离），以便在必要时进行更正。与A_star一样，LPA* 使用启发式算法，该启发性来源于从给定节点到目标路径代价的更低边界。如果保证是非负的（零可以接受）并且从不大于到目标的最低路径的代价，则允许该启发式。

• 启发式搜索和增量式搜索的区别
  • 启发式搜索是利用启发函数来对搜索进行指导，从而实现高效的搜索，启发式搜索是一种“智能”搜索，典型的算法例如A_star算法、遗传算法等。
  • 增量搜索是对以前的搜索结果信息进行再利用来实现高效搜索，大大减少搜索范围和时间，典型的例如LPA_star、D_star Lite算法等。

主要概念

• 父代节点与子代节点
  除了开始节点和目标节点外，每个节点n都有上一代和下一代节点：
  

  任何一条边指向n的节点都是n的前一代；
  从n引出一条边的任何节点都是n的后继节点。
  仅指直接的父代（上一代）和子代（下一代）

• 起始距离估计
  LPA_star对每个节点的起始距离g_star(n)保持两个估计值：
  
  • g(n)为之前计算的g值(起始距离)，表示；
  • rhs（n），一个基于节点父代（上一代）的g值的预测值（所有g（n’）+d（n’，n）的最小值，其中n’是n的父代，d（x，y）是连接x和y的边的代价）。
  • 对于开始节点，以下始终为真：
    rhs(start)=g(start)=0

如果rhs(n)等于g(n)，则称n局部一致的。如果所有节点都是局部一致的，那么最短路径可以用A* 来确定。
当边缘代价发生变化时，只需要对与路由相关的节点重新建立局部一致。

• 优先队列
  当一个节点在局部变得不一致时(因为它的父代节点的代价或将它链接到父代节点的边缘发生了变化)，它将被放在一个优先级队列中进行重新评估。
  LPA* 使用二维key：
  
  元素由$k1$(它直接对应于A* 中使用的f值)排序，然后由$k2$排序。

根据更小的k(n)的值搜寻更优的路径节点，当一个节点n比另一个节点n’更优时，n节点的k(n)不大于k(n’)，即：

节点状态及扩展

队列顶部节点展开如下：

局部一致（Locally Consistent）：g(s)=rhs(s)。当所有节点均为局部一致状态时，g(s)的值等于s到起始点的最短距离（注意，反向不成立）。这个概念很重要，当上述条件满足时，可以找到任意一点u到起始点的最短路径，假设当前位置为s,父辈节点s’(向着起始点前进的下一个节点)通过最小化(g(s’)+c(s,s’))来获得，不断重复直到到达sStart。然而，LPA*并不需要使所有节点均为局部一致状态，它通过启发式搜索将关注点放在搜索上，并且只更新那些与计算最短路径相关的节点的g值。
局部过一致（Locally Overconsistent）：g(s)>rhs(s)。当优先队列U中取出的节点为局部过一致状态时，意味着g(s)可以通过父辈节点使自己到起点的路径更短，此时将设置g(s)=rhs(s)，节点状态变为局部一致状态。
局部欠一致(Locally Underconsistent)：g(s)<rhs(s)。这种情况通常出现在父辈的某一节点突然变为障碍的情况下，造成父辈节点到起点的路径变大，从而需要修改g(s)的值，如果节点处于这种状态，则当它由优先队列中取出时，将其g值设置为∞，即将该节点状态变为局部过一致，而局部过一致的点将会被再次添加到优先队列中，这样就可以在它下次被取出时将其作为局部过一致状态处理，最终达到局部一致状态（如果这一节点与要搜索的最短路径相关的话）。

由于更改节点的g值也可能更改其后续节点的rhs值(从而更改它们的局部一致)，因此将对它们进行评估，并在必要时更新它们的队列成员和key。

节点将继续扩展到下一个节点，直到满足以下两个条件:

目标是局部一致的
优先级队列顶部的节点有一个大于或等于目标的key

初始化运行

当边的代价发生变化时，LPA*检查受变化影响的所有节点，即其中一条变化边终止的所有节点(如果一条边可以在两个方向上遍历，且变化影响两个方向，则检查由该边连接的两个节点)：

更新节点的rhs值。
已成为局部一致的节点将从队列中删除。
已成为局部不一致的节点将添加到队列中。
保持局部不一致的节点的key已更新。

之后，节点继续扩展，直到达到结束条件。

最短路径搜索

节点展开完成后(即满足退出条件)，计算最短路径。如果目标的代价为无穷大，那么从开始到目标没有有限的代价路径。否则，最短路径可以通过向后移动来确定：

从目标开始。
移动到当前节点n的父代n’，其中g（n’）+d（n’，n）最低（如果最低分数由多个节点共享，则每个节点都是有效的解决方案，其中任何一个都可以任意选择）。
重复上一步，直到开始。

伪代码

特点

• 对于LPA的每次运行，每个节点最多展开(访问)两次。每个LPA运行最多扩展一次局部过一致节点，因此其初始运行(每个节点进入过一致状态)的性能与A* 类似，都是最多访问每个节点一次。
• 每次运行时展开的节点key都是单调的不随时间递减的，就像A* 的情况一样。
• 启发式信息越丰富(因而越大)(同时仍然满足可接受性标准)，需要扩展的节点就越少。
• 优先级队列实现对性能有重大影响，如在A* 中。与效率较低的实现相比，使用斐波那契堆可以显著提高性能。

举例

第二次搜索：当场景中地形状态发生变动，在该例子中，节点(D,1)变为障碍，只改变了三个起始距离，即单元D1、E1和F0。这允许LPA*有效地重新规划最短路径，即使从开始单元到目标单元的最短路径完全改变。

这是重用以前的规划构建过程的一部分（以g值的形式），而不是以更大的内存需求为代价来调整先前规划的优点。特别是，g值不仅可以用来确定最短路径，而且它们比最短路径本身更容易再次使用。

首先对该节点进行更新，并将其后代节点置于优先队列中，该节点的变动对父辈节点的状态并无影响。同第一次搜索类似，不断进行迭代直到再次找到到目标位置的最短路径。

总结

LPA_star提出了一种解决动态环境下的最短路径搜索方法，但是它针对的是起始点和目标点固定的情况，如果在机器人按照搜索到的最短路径行走过程中，环境中某些节点发生变化，这时如果想获得新的路径，就得以机器人当前节点为起始点，重新用LPA_star算法解算一次，这效率是很低的。针对这种情况，该论文的作者随后提出了D_starLite算法来作为处理动态环境最短路径问题的高效方法。

https://blog.csdn.net/lqzdreamer/article/details/85175372

D*

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

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

符号说明

论文中将地图中的路径点用State表示，每一个State包含如下信息：

• Backpointer: 指向前一个state的指针，指向的state为当前状态的父辈，当前state称为指针指向state的后代，目标state无Backpointer——反向的父节点。
  （路径搜索完毕后，通过机器人所在的state，通过backpointer即可一步步地移动到目标Goal state，GoalState以后用 G表示），b（X）=Y表示X的父辈为Y。
• Tag：表示当前state的状态，有 New、Open、Closed三种状态，New表示该State从未被置于Openlist中，Open表示该State正位于OpenList中，Closed表示已不再位于Openlist中。
• H（X）：代价函数估计，表示当前State到G的开销估计。
• K（X）：Key Function，该值是优先队列Openlist中的排序依据，K值最小的State位于队列头 ，对于处于OpenList上的State X，K（X）表示从X被置于Openlist后，X到G的最小代价H（X），可以简单理解为。K（X）将位于Openlist的State X划分为两种不同的状态，一种状态为Raise（如果K（X）<H（X）），用来传递路径开销的增加（例如某两点之间开销的增加，会导致与之相关的节点到目标的路径开销随之增加）；另一种状态为 Lower（如果K（X）<H（X）），用来传递路径开销的减少（例如某两点之间开销的减少，或者某一新的节点被加入到Openlist中，可能导致与之相关的节点到目标的路径开销随之减少）。
• kmin：表示所有位于Openlist上的state的最小K值。
• C（X,Y） ：表示X与Y之间的路径开销。
• Openlist 是依据K值由小到大进行排序的优先队列。

算法描述

搜索的关键是state的传递过程，即由G向机器人所在位置进行搜索的过程，这种传递过程是通过不断地从当前OpenList中取出K值最小的State来实现的，每当一个State由Openlist中移出时，它会将开销传递给它的邻居state，这些邻居state会被置于Openlist中，持续进行该循环，直到机器人所在State的状态为 Closed ，或者Openlist为空（表示不存在到G的路径）。

算法最主要的是两个函数， Process-State 和 Modify-Cost ，前者用于计算到目标G的最优路径，后者用于改变两个state之间的开销C（X,Y）并将受影响的state置于Openlist中。

• 算法的主要流程
  在初始时，所有state的t（Tag）被设置为 New ，H（G）被设置为0，G被放置于OpenList，然后Process-State函数被不断执行，直到机器人所处state X由openlist中出队，然后可以通过机器人的当前state按backpointer指向目标G。当移动过程中发现新探测到的障碍时，Modify-Cost函数立刻被调用，来更正C（°）中的路径开销并将受影响的state重新置于openlist中。令Y表示robot发现障碍时所在的state，通过不断调用Process-State直到kmin≥H（Y），这时表示路径开销的更改已经传播到了Y，此时，新的路径构建完成。
  
• L1-L3表示拥有最低K值的X由openlist中移出，如果X为Lower，那么它的路径代价为最优的。
• 在L8-L13，X的所有邻接state都被检测是否其路径代价可以更低，状态为New的邻接state被赋予初始路径开销值，并且开销的变动被传播给每一个backpointer指向X的邻接state Y（不管这个新的开销比原开销大或者小），也就是说只要你指向了X，那么X的路径开销变动时，你的路径代价必须随之改变。这里可能存在由于X路径开销变动过大，Y可以通过非X的其他state到达目标且路径开销更小的情况，这点在L8-13中并没有处理，而是放在后续针对Y的process-state函数中，在对Y进行处理时，会将其backpointer指向周围路径开销最小的state。如果X的邻接State状态为New时，应将其邻接state的backpointer指向X。所有路径开销有所变动的state都被置于Openlist中进行处理，从而将变动传播给邻接的state。
• 如果X为Raise，它的路径开销H可能不是最优的
  
  • 在L4-L7中，通过其邻居state中已经处于最优开销（即h（Y）≤kold）的节点来优化X的路径开销，如果存在更短的路径，则将X的backpointer指向其neighbor。
  • 在L15-L18中，开销变动传播到状态为New的邻居state。如果X可以使一个backpointer并不指向X的邻居state的路径开销最小，即Y通过X到目标G的距离更短，但是此时Y的backpointer并不指向X，针对这种情况，可以将X重新置于Openlist中进而优化Y。在L23-25中，如果X可以通过一个状态为closed的并不是最理想的邻居stateY来减小路径开销，那么将Y重新置于Openlist中。

在modify-cost中，更新C（X,Y）并将X重新置于Openlist中，当X通过process-state进行传播时，会对Y进行开销计算，h（Y）=h(X)+c(X,Y)。

算法总结

相比A-star算法，D-star的主要特点就是由目标位置开始向起始位置进行路径搜索，当物体由起始位置向目标位置运行过程中，发现路径中存在新的障碍时，对于目标位置到新障碍之间的范围内的路径节点，新的障碍是不会影响到其到目标的路径的。新障碍只会影响的是物体所在位置到障碍之间范围的节点的路径。在这时通过将新的障碍周围的节点加入到Openlist中进行处理然后向物体所在位置进行传播，能最小程度的减少计算开销。

路径搜索的过程我个人感觉其实和Dijkstra算法比较像，A-star算法中f(n)=g(n)+h(n)，h(n)在D-star中并没有体现，路径的搜索并没有A-star所具有的方向感，即朝着目标搜索的感觉，这种搜索更多的是一种由目标位置向四周发散搜索，直到把起始位置纳入搜索范围为止，更像是Dijkstra算法。

在网上找到了一个用java写的D-star的源码，分享给大家，帮助大家理解。
链接：https://pan.baidu.com/s/1G1UtCe_1H8UhNe4hRdFl6A 密码：yta2

D* Lite算法

• 基于LPA_star 算法基础上提出的路径规划算法。
• D_star Lite 采用反向搜索方式
• 到LPA_star 算法还是A_star 算法都不能满足移动机器人在未知环境中的路径规划需
  因为其在未知地图中需要不断的尝试，与边走边找到最优路径背道而驰。此时反向搜索算法能够很好的处理这种情况
• D_star 算法效率较低
• D_star Lite算法核心在于假设了未知区域都是自由空间，增量式地实现路径规划，通过最小化rhs值找到目标点到各个节点的最短距离。
• 把移动机器人前进时其所到的节点即设置为起始节点，因此路径变化或者key值需要更新时，需要更新从目标点到新起点的启发值以及估计成本。由于移动机器人不断的靠近目标点，节点的启发值将不断减少，且减少至不会超过h（start Org,start New）。由于每次都要减去相同的值，开启列表的顺序并不会改变，因此可以不进行这部分的计算，这样便避免了每次路径改变时的队列遍历过程。
• 若前行过程中发现障碍物则将障碍物所对应环境地图位置设置为障碍物空间，并再以之为起点利用“路径场”信息重新规划出一条路径来。此时不仅更新规划路径的节点数据，也要更新智能体遍历过的节点。其关键点在于如何在未知的环境中根据传感器获取的极少周边地图信息来进行最有效的靠近目标点的任务。其实整个靠近的过程一直在扩大已知地图范围，尽可能少的尝试次数来实现完成抵达目标点的任务。下图为 D_star Lite 搜索示意图，黑点是在按照反向搜索的路径执行时发现的障碍点，到遇到不能通行的障碍点后便更新地图信息，重新规划出一条新的路径继续前行。

算法介绍

• D_star Lite 算法的原理
  起初需要根据已知的环境信息，未知部分视作自由空间，规划出从目标点到起始点的全局最优路径，此时即建立了一个“路径场”信息，为增量靠近目标点提供择优依据。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算法相对应，可以很容易得出以下公式：

算法总结

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

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

D* 描述2

• D*
  算法虽然可以实现未知环境的路径规划，但效率较低，基于 LPA的 D Lite 可以很好的应对环境未知的情况，其算法核心在于假设了未知区域都是自由空间，以此为基础，增量式地实现路径规划，通过最小化 rhs值找到目标点到各个节点的最短距离
• D* Lite 算法的原理类似 D，起初需要根据已知的环境信息，未知部视作自由空间，规划出从目标点到起始点的全局最优路径，此时即建立了一个“路径场”信息，为增量靠近目标点提供择优依据。D Lite 算法是反向搜索的
• D* Lite 算法的不足
  当状态空间比较大，也就是环境地图比较大的时候，采用的 D* Lite 路径规划算法的反向搜索过程需要维护的栅格节点数急剧增加，加了搜索的时间复杂度。除此之外，D* Lite 路径规划不能应对环境复杂的情况，即局部环境的精细规划。对于大环境下的路径规划，D* Lite 算法的做法是将环境地图进行更细粒度的栅格化，虽然在足够细粒化的环境地图中可以实现较优的路径解，但也会带来更多的
  规划序列导致执行次数以及重规划次数增多，进而路径规划执行花费时间也会变得更长。比较好的解决方案是采用粗细结合的方案，即维护两种分辨率地图，上层地图采用粗粒度的高分辨栅格地图表示，下层即栅格单元采用细粒度的地分辨栅格分解。首先进行上层地图的 D* Lite 路径规划，在执行规划序列时必要的情况下加载下层栅格地图进行精细 D* Lite 路径规划。这样能够将规划问题进行分解，
  分为高层粗粒度路经规划与底层细粒度路径规划两部分，两层的路径规划的维护的地图栅格数有所降低，会带来一定程度的时间复杂度的降低。但是总的栅格节点仍然是那么多，并不能带来存储空间的优化，而且其又带来了结构复杂的问题。并且依然并不能解决局部环境下的精准细密的规划问题，即在复杂环境下不能根据环境特征进行慎思规划。
• The robot could use conventional graph-search methods but this is inefficient since most edge costs do not change between replanning episodes [4].
• D首先利用已知的、假设的或估计的耗费信息规划一条路经并开始沿路径向目标移动，在执行过程中每当获得新的耗费信息迅速重新规划。D算法是一个增量式的算法，它维护一个部分优化的耗费图，每当路径耗费发生变化时它将耗费的改变信息增量式地在图中状态进行传播，降低了计算的时间，能较好地满足实时应用。

参考

• 《Anytime Dynamic A*: An Anytime, Replanning Algorithm》论文 个人读后小结
  https://blog.csdn.net/sxt1001/article/details/80636270
• wiki D* 算法
  https://www.wikiwand.com/en/D*