（6-2-01）D* Lite 算法：D* Lite算法的核心思想（1）

6.2  D* Lite算法的核心思想

D* Lite算法通过在环境变化时仅对受影响的部分路径进行局部更新，以及利用A*算法的启发式搜索特性，实现了在动态环境中高效进行路径规划的能力。

6.2.1  基本思想

D* Lite算法的基本思想是在动态环境中进行增量式的路径规划。它的设计灵感主要来自于D算法，旨在解决在动态环境中进行路径规划时遇到的问题。D* Lite算法的基本思想如下所示。

1. 增量式路径更新：D* Lite算法不是重新规划整条路径，而是在环境发生变化时仅更新受影响的部分路径。这种增量式的路径更新可以减少计算量，并使算法能够更快地适应环境的变化。
2. 局部搜索：当环境发生变化时，D* Lite算法通过对受影响的路径段进行局部搜索，以找到最优的路径更新。这种局部搜索的效率比重新规划整条路径要高得多，因为它只需要考虑受影响的部分路径。
3. 双向搜索：D* Lite算法采用双向搜索策略，在正向和反向两个方向上同时搜索路径。这种双向搜索可以加快路径搜索的速度，并且能够更快地找到最优路径。
4. 启发式信息：D* Lite算法利用启发式信息来指导路径搜索过程，以尽快找到最短路径。启发式信息可以帮助算法更有效地探索搜索空间，从而提高路径搜索的效率。
5. 动态调整代价值：D* Lite算法通过动态调整代价值来适应环境的变化。当环境发生变化时，算法会重新计算受影响路径段的代价值，并根据新的代价值更新路径。

综上所述，D* Lite算法的基本思想是通过增量式的路径更新和局部搜索，在动态环境中高效地进行路径规划。D* Lite算法利用双向搜索和启发式信息来加速路径搜索过程，并通过动态调整代价值来适应环境的变化。

6.2.2  D* Lite算法的实现步骤

D* Lite算法通过增量式的路径更新和局部搜索，在动态环境中实现高效的路径规划。其实现步骤主要包括路径更新、路径返回和循环迭代等。具体来说，D* Lite算法的实现步骤如下所示。

（1）初始化

1. 初始化起始状态和目标状态。
2. 设置初始路径为从起始状态到目标状态的最短路径（通常使用A*算法）。
3. 初始化代价图，记录每个网格的代价值。

（2）路径更新

1. 根据环境的变化，如障碍物的移动或出现，以及机器人的移动，需要更新路径。
2. 向目标搜索：从目标状态开始向起始状态搜索路径，并更新路径代价值。
3. 反向搜索：从起始状态开始向目标状态搜索路径，并更新路径代价值。
4. 比较路径：比较正向和反向搜索的路径，找到最短路径。
5. 更新代价值：根据新的路径更新代价图，以反映环境的变化。

（3）路径返回：根据更新的路径信息，机器人或者系统根据新的路径进行移动。

（4）循环迭代

1. 重复路径更新和路径返回步骤，随着环境的不断变化和机器人的移动，周期性地更新路径，并进行局部搜索。
2. 迭代直到达到终止条件（例如，机器人到达目标状态或者达到预设的最大迭代次数）。

（5）优化和终止条件

1. 根据具体应用情况添加一些优化策略，例如设置最大迭代次数、设定收敛条件等。
2. 当达到终止条件时，算法停止执行，并输出最终的路径结果。

在实现D* Lite算法时，需要考虑如何有效地表示地图、设计路径搜索算法、更新代价值、以及如何根据环境变化动态地调整路径。D* Lite算法的核心是增量式的路径更新和局部搜索，以及适应环境变化的能力。

6.2.3  D* Lite算法的基本概念

D* Lite 是一种增量启发式算法。启发式和 A * 类似，同样有一个启发函数，不过因为 D* Lite 是从终点向起点搜索，所以对应的启发函数 h(n) 也变成了节点 n 到起点的估计值。增量学习是指在学得模型后，再接收到训练样例时，仅需根据新样例对模型进行更新，不必重新训练整个模型，并且先前学得的有效信息不会被冲掉。D* Lite 算法就是这样，当检测到了新的障碍物，算法不需要完全重新规划路径，可以利用之前搜索所获得的信息，来找到一条可以避开障碍物的路径。

1. g 值 h 值以及 rhs 值

和 A* 不同，D* Lite 是从终点向起点搜索，所以 g(n) 和 h(n) 的定义也就有所不同：

1. g(n)：当前点到终点的实际代价。
2. h(n)：当前点到起点的估计值。

D* Lite 中引入了一个新的概念就是 rhs(right-hand side)，它的定义公式如下所示：

一个点的 rhs 值是它的父代节点中 g 值加上这两点之间的代价中的最小值，相当于一个点从父代节点到达这个点的最小代价。其实在算法的大部分过程中，g 值和 rhs 值是相等的。

2. 两个 key 值

在 A* 算法中，通过 f(n) 的大小来判断一个点的优先级。而在 D* Lite算法 中，需要通过两个 key 值来判断一个点的优先级，key 值越小优先级越高，先判断第一个 key 值，如果第一个 key 值相等再判断第二个 key 值。它们的公式如下：

其中 km的定义为，算法初始化时会先将 km设置为 0，之后当机器人有检测到地图的变化时，km+=h(slast,sstart)，slast代表的是上一个起点，sstart是机器人的当前位置。因为每当机器人有检测到地图的变化时，要计算两点之间的启发距离（不考虑障碍物），并且把当前所在的点设置为新的起点，即更新起点的位置。

我们一个个来看。首先是第一个 key，它由三项构成，当前点的 g 值和 rhs 值中的最小值加上当前点到起点的估计值在加上一个 km。简单的理解话：第一项就是到终点的实际距离，第二项是到起点的估计值，如果在机器人还没有移动的时候 km就等于 0，这时算法其实就相当于一个反向从终点往起点方向搜索的 A* 算法了。那么 km引入的意义是什么呢，当机器人检测到障碍的变化时会再一次规划路径，这时候的实际起点应该是机器人当前的位置，起点发生了变化，每个点的 h 值也会相应变化，key 值也发生了变化。如果不引入这个参数的话，就需要把优先队列中的全部节点都重新计算一遍 key 值，增加了计算量。引入之后就可以一定程度上保证 key 值的一致性，减少计算量。

第二 key 值就是 g 值和 rhs 值中的最小值，它的意义在于当两个点的第一个 key 值相等的时候，算法会优先选择距离终点近的点。

3. 局部一致性

在D* Lite 算法中还有一个很重要的概念就是局部一致性，通过一个点的局部一致性来判断当前点是否需要计算。其定义如下：当一个点的 g 值等于 rhs 值时称这个点为局部一致的点，否则称这个点为局部不一致。其中局部不一致的情况还可细分成为局部过一致和局部欠一致：当一个点的 g 值大于 rhs 值时，这个点为局部过一致，通常是有障碍物删除时或者算法第一次搜索路径时；当一个点的 g 值小于 rhs 值时，这个点为局部欠一致，通常是检测到了新增的障碍物。

6.2.4  伪代码

在Sven Koenig和Maxim Likhachev的论文中给出了D* Lite 算法的伪代码，接下来将介绍伪代码中的主要内容。

1. 主函数

下面是 D* Lite算法的 main 函数，它的思路是先计算最短路径，其最短路径是通过周围点的 g 值来判断的，机器人一直走向 g 值加上两点代价最小的点。如果检测到了障碍物的变换，那么更新受影响点的 rhs 值，然后重新计算最短路径。

对上述代码的具体说明如下所示。

{29} 首先是将机器人的当前点设置为起点。

{30} 然后调用 Initialize() 函数进行初始化，初始化的内容如下：

1. 初始化需要将优先队列设置为空队列；置为 0；
2. 将所有节点的 g 值和 rhs 值设置为无穷；
3. 最后将终点的 rhs 值设置为 0 并计算它的 key 值加入到优先队列中。

{31} 初始化之后就调用 ComputeShortestPath() 开始计算它的最短路径。

{32}~{48} 之后就开始循环，直到机器人到达了终点。循环开始后：

 {34}~{35} 先根据之前的计算将机器人移动到子代中 g 值加上这两个点之间的代价中最小的点。

 {36}~{48} 如果机器人检测到了障碍的变化：

  {38}~{39} 修改 k_m 值，根据上一个起点和当前点的启发值，并且将当前点设置为上一个起点。

  {40} 对于所有两个点之间的代价发生变化的：

   {41}~{42} 另然后更新这两个点之间的代价

   {43}~{44} 如果：代表两个点之间的代价变小，有障碍物删除。那么优化它的 rhs 值。

   {45}~{46} 这里的判定条件是：代表原来的路径可以通过这两个点，但是他们之间的代价发生了变化，一般可以理解成可能的路径上新增了一个障碍物。然后再通过点的子代来更新 rhs 值

   {47} 更新受影响的节点。

  {48} 计算最短路径。

2. 更新一个节点

下面是更新一个节点的伪代码：

对上述代码的具体说明如下所示。

{07} 如果当前点的 g 值和 rhs 值不相等，即为局部不一致，并且这个点在优先队列中，那么就更新这个点在优先队列中的 key 值。

{08} 如果一个点的 g 值和 rhs 值不相等，并且这个点不在优先队列中，那么就计算这个点的 key 值，并加入到优先队列中。

{09} 如果一个点 g 值和 rhs 值相等，并且还在优先队列中，那么就将其从优先队列中删除。

3. 计算最短路径

下面是计算最短路径的伪代码：

对上述代码的具体说明如下所示。

{10} 进行路径判断的条件是：优先队列中 key 值最小的 key 值要小于 “起点”（机器人所在的位置）的 key 值或者起点的 rhs 值大于它的 g 值：

 {11}~{13} 取得优先队列中优先值最高即 key 值最小的节点和它的 key 值，并重新计算它的 key 值。

 {14}~{15} 如果它的新的 key 值大于它旧的 key 值那么就将它和它新的 key 值重新放到优先队列中。

 {16} 如果当前节点的 g 值大于 rhs 值（通常是第一次搜索或者有障碍物的删除时）：

  {17}~{18} 另当前点的 g 值等于 rhs 值，并且将其从优先队列中移除。

  {19}~{21} 更新当前节点的父节点（且不是终点），如果可以让它们的 rhs 值取得更小的值，那么就更新它们的 rhs 值。最后调用 UpdateVertex 来将它们加入到优先队列中。

 {22} 否则（当前节点的 g 值小于等于 rhs 值，一般是有新增加的障碍物）：

  {23}~{24} 记录当前节点的 g 值为之后将其置为无穷。

  {25} 对于所有的当前点的父节点以及当前点：

   {26}~{28} 如果它们（{25} 中的点）的 rhs 值等于加上两点间的代价，那么就从它的子节点来更新 rhs 值，最后再调用 UpdateVertex 来更新它们。