路径搜索算法——D*Lite算法

原创已于 2025-07-31 12:13:42 修改 · 1.3k 阅读

30 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #网络

于 2025-07-02 16:43:01 首次发布

路径搜索算法专栏收录该内容

8 篇文章

订阅专栏

路径搜索算法——D*Lite算法

一、**D*Lite算法简介**

DLite算法是一种用于路径规划的增量搜索算法，特别适用于动态环境。它是对经典A算法的改进，能够在地图发生变化时高效地更新路径，而无需从头重新计算整个路径。D*Lite通过维护一个优先级队列来选择需要扩展的节点，并利用启发式函数来指导搜索过程，从而实现高效的路径规划。

核心特点

D*Lite算法的关键优势包括：

增量更新：当环境发生局部变化（例如障碍物出现或消失）时，它能够快速调整已有路径，而不需要重新规划全局。
高效性：通过重用之前的搜索信息，D*Lite显著减少了计算开销。
适应性：非常适合动态或未知环境下的路径规划任务。

工作原理

DLite算法基于A算法的基础思想，但它特别优化了在动态环境中的表现。它利用优先级队列管理待扩展的节点，并结合启发式函数评估路径成本。当环境变化时，D*Lite会根据先前的搜索结果快速更新受影响的区域，从而生成新的最优路径。

二、算法核心思想

以下是对 DLite 算法具体细节的详细介绍。DLite 是一种用于路径规划的增量搜索算法，特别适合动态环境，能够高效地更新路径而无需从头重新计算。下面将从节点属性、优先级队列、更新规则以及路径提取等方面展开具体细节的说明。

1. 节点属性

在 D*Lite 算法中，每个节点都维护了一些关键属性，这些属性是算法运行的核心：

$g （ s ）$ ：表示从起点到该节点的实际成本。初始时，起点 $g$ 值为 0，其他节点的 $g$ 值设为无穷大。随着搜索的进行， $g$ 值会根据实际路径成本逐步更新。
$r h s (s) = s' \in S u cc (s) min [c (s, s') + g (s')]$ ：基于邻居的估计成本，具体计算方式为$ rhs = min(g(邻居) + 边成本)$，即从所有邻居中选择一条代价最小的路径到该节点。它反映了当前节点基于已有信息的最优估计成本。
$h (s)$ **：启发式函数，估计从该节点到终点的成本，通常基于某种距离度量（如欧几里得距离或曼哈顿距离）。 $h$ 值必须满足**三角不等式，以保证算法的正确性。

启发函数	适用场景	说明
曼哈顿距离（Manhattan）	仅水平垂直移动的栅格地图	$h(s) =∣x_1−x_2∣+∣y_1−y_2∣$
欧几里得距离（Euclidean）	可以斜着走的平面图	$\sqrt{(x_1 - x_2)^2 + (y_1 - y_2)^2}$
对角线距离	允许八方向移动	$(\Delta x + \Delta y) + (\sqrt{2} - 2) \cdot \min(\Delta x, \Delta y)$

这些属性的核心在于通过比较 $g (s)$ 和 $r h s (s)$ 的关系，判断一个节点是否需要更新。如果 $g \neq = r h s$ ，说明该节点的成本信息不一致，需要进一步处理。

D* Lite 算法的一个关键特征就是：

✅ 它是从目标（终点）向起点方向反向计算路径代价的。

对比项	A* / Dijkstra	D* Lite
搜索方向	起点 → 终点	终点 → 起点 ✅
路径执行	找到后一步步前进	找到后按 $g (s)$ 最小走
路径更新	必须重跑	可以增量修复 ✅
启发函数计算	$h (s, 终点)$	$h (起点, s)$ （反着算）✅

2. 优先级队列

D*Lite 使用优先级队列来管理待扩展的节点，确保算法始终处理最有希望的节点。优先级队列中的每个节点都有一个由两部分组成的 key 值，用于排序：

$k ey (s) = [k 1, k 2] = [min (g (s), r h s (s)) + h (s t a r t, s) + km, min (g (s), r h s (s))]$

$k_1 = min(g, rhs) + h$ ：第一优先级，表示从起点经过该节点到终点的估计总成本。
$k_2 = min(g, rhs)$ ：第二优先级，用于在 $k_1$ 相同时进一步比较。
而 $km$ 是累积偏移量，用来补偿起点发生移动的历史；

排序规则：

优先级队列首先选择 $k_1$ 最小的节点。
如果多个节点的 $k_1 $值相同，则选择 $k_2$ 最小的节点。

这种双重优先级设计确保了算法能够优先处理那些可能对最优路径产生影响的节点，从而提高效率。

队列的作用：

初始化时，将起点加入优先级队列。
每次从队列中取出优先级最低的节点，更新其邻居的 $r h s (s)$ 值，并根据需要将邻居加入队列。
当环境发生变化时，受影响的节点也会被加入队列，触发增量更新。

总结：

公式	含义	举例说明
$g (s)$	已知从目标到当前节点的路径代价	我已经知道的从目标到 s 的最短路径
$r h s (s)$	通过邻居的建议来计算从当前节点到目标的代价	邻居告诉我“你可以走这条路，最短的是这条”
$k ey (s)$	排序标准，确定哪个节点最紧急处理	按照谁更急，需要先处理哪个节点
$r h s (s) = min (c (s, s^{'}) + g (s^{'}))$	从邻居的最佳路径建议来更新当前节点	我根据邻居的估算值来更新自己的路径建议
$g (s) = r h s (s)$	只要路径一致，说明已经找到了最短路径	如果 $g $和$ rhs$ 相等，说明路径不再需要更新

3. 更新规则

D*Lite 的增量更新机制是其高效性的关键。更新规则主要围绕 $g (s)$ 和 $r h s (s)$ 值展开：

一致性检查：
- 如果一个节点的 $g (s)$ 值等于 $r h s (s)$ 值（ $g = r h s$ ），则该节点是“一致的”，无需更新。
- 如果 $g \neq = r h s$ ，则该节点是“不一致的”，需要加入优先级队列进行处理。
  - $g > r h s$ ：表示当前成本过高，可能存在更优路径。
  - $g < r h s$ ：表示当前成本过低，可能需要调整以反映新的环境变化。
邻邻更新：
- 当一个节点的 $r h s (s)$ 值因环境变化或邻居更新而改变时，算法会检查该节点的所有邻居。
- 对每个邻居，重新计算其 $r h s (s)$ 值（基于新的 $min (g (邻居) + 边成本)$ ）。
- 如果邻居的 $r h s (s)$ 值发生变化，且变得不一致（ $g \neq = r h s$ ），则将其加入优先级队列。
动态调整：
- 当环境中出现新障碍物或移除障碍物时，只需更新受影响节点的边成本，重新计算相关节点的 $r h s (s)$ 值，并将这些节点加入优先级队列。
- 算法会自动传播这些变化，最终生成新的最优路径。

这种局部更新机制避免了全局重新计算，使得 D*Lite 在动态环境中表现出色。

4. 路径提取

当路径规划完成后，D*Lite 通过以下方式提取最优路径：

终止条件：
- 当终点的 $g (s)$ 值等于 $r h s (s)$ 值（ $g = r h s$ ），且优先级队列为空时，说明路径已稳定，搜索完成。
回溯过程：
- 从终点开始，沿着 $r h s (s)$ 值回溯。
- 对于当前节点，选择一个邻居，使得$ g(邻居) + 边成本 = rhs(当前节点)$。
- 重复此过程，直到回到起点，得到完整的路径。

这种方法利用了 $r h s (s)$ 值反映最优路径成本的特性，确保提取的路径是最优的。

5.总结

D* Lite 的核心逻辑是：从终点反向规划路径、从起点正向执行路径，并能在环境变化时高效地增量修复路径。

D* Lite 的“初始化搜索阶段”确实是从终点出发向起点反向传播路径信息，用来填充每个格子的 rhs（预期路径代价），直到 start.g == start.rhs。这代表起点也“收到了”目标的最短路径估计。

路径执行阶段就是机器人从 start 出发，每次选择邻居中 $r h s (s)$ 最小的格子（即当前预估最短路径），逐步向目标推进。直到走到目标点（rhs == 0）为止。

D* Lite 的最大优点就在于：

当机器人感知环境有变化（如新增障碍），它只需对受影响区域的节点调用 UpdateVertex()；
然后再次调用 ComputeShortestPath()，就可以快速修复路径（不是完全重算！）；
此时仍然是从目标反向传播路径估值，直到起点再次“收敛”。

D* Lite 关注的是 图中边的代价变化（edge cost change），比如：

新的障碍出现（某条边从可通变不可通 → $g (s) = \infty$ ）；
障碍移除（某条边从不可通变可通 → $g (s) = 1$ ）；
路面代价发生改变（从 1 → 5 等）。

D* Lite 不需要处理整张图，只需要处理这些变化边的两端节点。

假设 (x, y) 是新增障碍：

它原本是起点某条可通路径上的邻居；
那么就要更新 它的所有相邻节点（即：有 $g (s)$ 涉及(x, y)的所有s`）；
也就是把这些节点加入 open_list，并重新调用 UpdateVertex(s)。

三、无更新例子说明

假设我们有一个5行4列的网格，共20个单元格，编号从1到20，布局如下：

在这里插入图片描述

网格和移动规则

这是一个5x4的网格，假设移动规则是四连通的，即从每个单元格只能移动到上、下、左、右的相邻单元格，且每次移动的代价为1。障碍物单元格6和8不能被穿过，因此在规划路径时需要绕过它们。

使用D*Lite算法

根据要求，我们将使用DLite算法来规划路径。DLite是一种增量式搜索算法，适用于动态环境，但在此静态环境中也可以有效工作。它通过维护每个单元格的两个值—— $g$ 值（从起点到该单元格的当前估计代价）和 $r h s$ 值（基于邻居的一步展望代价）——以及一个优先级队列来找到最优路径。

初始化

对于所有单元格s，设置 $g (s) = \infty ， r h s (s) = \infty$ 。
将终点单元格16的 $r h s$ 值设为0，即 $r h s (16) = 0$ 。
计算单元格16的键值并将其加入优先级队列。键值是一个二元组 $k_1, k_2]$ ，其中：
- $k 1 = min (g (s), r h s (s)) + h (s)$
- $k 2 = min (g (s), r h s (s))$
- 启发式函数 $h (s)$ 是从单元格 $s$ 到起点的曼哈顿距离，即 $h(s) = |row_s - 1| + |col_s - 1|$ 。
- 对于单元格16 $（ 4, 4 ）$ ， $h (16) = ∣4 - 1∣ + ∣4 - 1∣ = 6$ ， $min (g (16), r h s (16)) = min (\infty, 0) = 0$ ，所以键值为 $[0 + 6, 0] = [6, 0]$ 。

主循环

算法从优先级队列中取出键值最小的单元格，更新其 $g$ 值和邻居的 $r h s$ 值，直到起点单元格的 $g$ 值和 $r h s$ 值一致，且队列为空或无需进一步优化。以下是简化的执行过程：

处理单元格16
- 队列： ${16:[6,0]}$
- $g (16) = \infty > r h s (16) = 0 ，更新 g (16) = 0 。$
- $邻居（前驱）： 12 （上）、 15 （左）、 20 （下），无右邻居。$
- 更新 $r h s$ 值（假设移动代价 $c = 1$ ）：
  - $r h s (12) = min (\infty, 1 + g (16)) = 1 ，键值 [1 + h (12), 1] ， h (12) = ∣3 - 1∣ + ∣4 - 1∣ = 5 ， [6, 1]$ 。
  - $r h s (15) = 1 ， h (15) = ∣4 - 1∣ + ∣3 - 1∣ = 5 ，键值 [6, 1] 。$
  - $r h s (20) = 1 ， h (20) = ∣5 - 1∣ + ∣4 - 1∣ = 7 ，键值 [8, 1] 。$
- 队列： ${12:[6,1], 15:[6,1], 20:[8,1]}。$
处理单元格15
- 队列： ${15:[6,1], 12:[6,1], 20:[8,1]}$
- $g (15) = \infty > r h s (15) = 1 ，更新 g (15) = 1 。$
- 邻居：14（左）、11（上）、19（下）、16（右）。
- 更新：
  - $r h s (14) = min (\infty, 1 + 1) = 2 ， h (14) = ∣4 - 1∣ + ∣2 - 1∣ = 4 ，键值 [6, 2] 。$
  - $r h s (11) = min (\infty, 1 + 1) = 2 ， h (11) = ∣3 - 1∣ + ∣3 - 1∣ = 4 ，键值 [6, 2] 。$
  - $r h s (19) = 2 ， h (19) = ∣5 - 1∣ + ∣3 - 1∣ = 6 ，键值 [8, 2] 。$
  - $r h s (16) = 0 ，不变。$
- 队列： ${12:[6,1], 14:[6,2], 11:[6,2], 20:[8,1], 19:[8,2]}。$
处理单元格12
- $g (12) = \infty > r h s (12) = 1 ，更新 g (12) = 1 。$
- 邻居：8（上，障碍）、11（Shoot）、16（下）。
- 更新：
  - $r h s (11) = min (2, 1 + 1) = 2 ，无变化。$
  - $r h s (16) = 0 ，不变。$
- 队列： ${14:[6,2], 11:[6,2], 20:[8,1], 19:[8,2]}。$
处理单元格14
- $g (14) = \infty > r h s (14) = 2 ，更新 g (14) = 2 。$
- 邻居：13（左）、10（上）、15（右）。
- 更新：
  - $r h s (13) = min (\infty, 1 + 2) = 3 ， h (13) = ∣4 - 1∣ + ∣1 - 1∣ = 3 ，键值 [6, 3] 。$
  - $r h s (10) = 3 ， h (10) = ∣3 - 1∣ + ∣2 - 1∣ = 3 ，键值 [6, 3] 。$
  - $r h s (15) = 1 ，不变。$
- 队列： ${11:[6,2], 13:[6,3], 10:[6,3], 20:[8,1], 19:[8,2]}。$
处理单元格11
- $g (11) = \infty > r h s (11) = 2 ，更新 g (11) = 2 。$
- 邻居：7（上）、10（左）、12（右）、15（下）。
- 更新：
  - $r h s (7) = 3 ， h (7) = ∣2 - 1∣ + ∣3 - 1∣ = 3 ，键值 [6, 3] 。$
  - $r h s (10) = min (3, 1 + 2) = 3 ，不变。$
  - $r h s (12) = 1 ，不变。$
  - $r h s (15) = 1 ，不变。$
- 队列： ${13:[6,3], 10:[6,3], 7:[6,3], 20:[8,1], 19:[8,2]}。$
处理单元格13
- $g (13) = \infty > r h s (13) = 3 ，更新 g (13) = 3 。$
- 邻居：9（上）、14（右）。
- 更新：
  - $r h s (9) = min (\infty, 1 + 3) = 4 ， h (9) = ∣3 - 1∣ + ∣1 - 1∣ = 2 ，键值 [6, 4] 。$
  - $r h s (14) = 2 ，不变。$
- 队列： ${10:[6,3], 7:[6,3], 9:[6,4], 20:[8,1], 19:[8,2]}。$
处理单元格10
- $g (10) = \infty > r h s (10) = 3 ，更新 g (10) = 3 。$
- 邻居：9（左）、6（上，障碍）、11（右）。
- 更新：
  - $r h s (9) = min (4, 1 + 3) = 4 ，无变化。$
  - $r h s (11) = 2 ，不变。$
- 队列： ${7:[6,3], 9:[6,4], 20:[8,1], 19:[8,2]}。$
处理单元格7
- $g (7) = \infty > r h s (7) = 3 ，更新 g (7) = 3 。$
- 邻居：3（上）、6（左，障碍）、8（右，障碍）、11（下）。
- 更新：
  - $r h s (3) = min (\infty, 1 + 3) = 4 ， h (3) = ∣1 - 1∣ + ∣3 - 1∣ = 2 ，键值 [6, 4] 。$
  - $r h s (11) = 2 ，不变。$
- 队列： ${9:[6,4], 3:[6,4], 20:[8,1], 19:[8,2]}。$
处理单元格9
- $g (9) = \infty > r h s (9) = 4 ，更新 g (9) = 4 。$
- 邻居：5（上）、10（右）、13（下）。
- 更新：
  - $r h s (5) = min (\infty, 1 + 4) = 5 ， h (5) = ∣2 - 1∣ + ∣1 - 1∣ = 1 ，键值 [6, 5] 。$
  - $r h s (10) = 3 ，不变。$
  - $r h s (13) = 3 ，不变。$
- 队列： ${3:[6,4], 5:[6,5], 20:[8,1], 19:[8,2]}。$
处理单元格3
- $g (3) = \infty > r h s (3) = 4 ，更新 g (3) = 4 。$
- 邻居：2（左）、4（右）、7（下）。
- 更新：
  - $r h s (2) = 5 ， h (2) = ∣1 - 1∣ + ∣2 - 1∣ = 1 ，键值 [6, 5] 。$
  - $r h s (4) = 5 ， h (4) = ∣1 - 1∣ + ∣4 - 1∣ = 3 ，键值 [8, 5] 。$
  - $r h s (7) = 3 ，不变。$
- $队列：{5:[6,5], 2:[6,5], 20:[8,1], 19:[8,2], 4:[8,5]}$ 。
处理单元格5
- $g (5) = \infty > r h s (5) = 5 ，更新 g (5) = 5 。$
- $邻居： 1 （上）、 6 （右，障碍）、 9 （下）。$
- 更新：
  - $r h s (1) = min (\infty, 1 + 5) = 6 ， h (1) = ∣1 - 1∣ + ∣1 - 1∣ = 0 ，键值 [6, 6] 。$
  - $r h s (9) = 4 ，不变。$
- 队列： ${2:[6,5], 1:[6,6], 20:[8,1], 19:[8,2], 4:[8,5]}。$
处理单元格2
- $g (2) = \infty > r h s (2) = 5 ，更新 g (2) = 5 。$
- 邻居：1（上）、3（右）。
- 更新：
  - $r h s (1) = min (6, 1 + 5) = 6 ，无变化。$
  - $r h s (3) = 4 ，不变。$
- 队列： ${1:[6,6], 20:[8,1], 19:[8,2], 4:[8,5]}。$
处理单元格1
- $g (1) = \infty > r h s (1) = 6 ，更新 g (1) = 6 。$
- 邻居：2（下）、5（右）。
- 更新：
  - $r h s (2) = 5 ，不变。$
  - $r h s (5) = 5 ，不变。$
- 此时 $g (1) = r h s (1) = 6$ ，起点一致。

算法继续处理队列中的剩余单元格，但起点已一致，无需进一步扩展即可回溯路径。

路径回溯

从起点单元格1开始，选择rhs值最小的邻居逐步到达终点：

单元格1： $r h s (2) = 5, r h s (5) = 5$ ，可选择2或5，选5（更靠近终点）。
单元格5： $r h s (1) = 6, r h s (9) = 4$ ，选9。
单元格9： $r h s (5) = 5, r h s (10) = 3, r h s (13) = 3$ ，选13。
单元格13： $r h s (9) = 4, r h s (14) = 2$ ，选14。
单元格14： $r h s (13) = 3, r h s (15) = 1$ ，选15。
单元格15： $r h s (14) = 2, r h s (11) = 2, r h s (19) = 2, r h s (16) = 0$ ，选16。
到达单元格16。

最终路径

从单元格1到单元格16的最优路径为：
1 → 5 → 9 → 13 → 14 → 15 → 16
路径长度为6（共6步），避开了障碍物单元格6和8。

验证

1 (1,1) → 5 (2,1)：向下，合法。
5 (2,1) → 9 (3,1)：向下，合法。
9 (3,1) → 13 (4,1)：向下，合法。
13 (4,1) → 14 (4,2)：向右，合法。
14 (4,2) → 15 (4,3)：向右，合法。
15 (4,3) → 16 (4,4)：向右，合法。
未经过单元格6 (2,2) 或 8 (2,4)。

在这里插入图片描述

因此，在这个5x4网格中，从单元格1到单元格16的最优路径是 1 → 5 → 9 → 13 → 14 → 15 → 16。

四、更新例子说明

在这里插入图片描述

D* Lite 动态路径修复示例：单元格 14 被堵住

假设我们仍然使用前述 5x4 网格，总共有 20 个编号为 1 到 20 的单元格。原始障碍为 6 和 8，原规划路径为：

1 → 5 → 9 → 13 → 14 → 15 → 16

此时，当机器人沿着该路径走到单元格 13 后，意外发现 14 成为障碍物（例如前方突发障碍），需要使用 D* Lite 的增量路径修复机制进行动态重新规划。

网格和移动规则

路径执行至单元格13时环境变化

机器人正沿着路径前进至单元格13，此时探测到前方单元格14变为障碍，不能再通行。

更新操作如下：

将单元格14标记为障碍，令其不可达。
所有边 c(s,14) 和 c(14,s') 的代价更新为 ∞。
执行 UpdateVertex() 的节点包括14自身及其邻居节点：13、10、15、18。
每个受影响节点重新计算：
- $min_{s' \in Succ(s)} [c(s,s') + g(s')]$
- $k ey (s) = [min (g (s), r h s (s)) + h (s t a r t, s) + km, min (g (s), r h s (s))]$
- 若 $\ne rhs(s)$ ，则重新加入优先队列。

初始状态（到达13前）

初始路径1 → 5 → 9 → 13 → 14 → 15 → 16的g值（假设终点 $g (16) = 0$ ，反向计算）：

$g (16) = 0$
$g (15) = 1$
$g (14) = 2$
$g (13) = 3$
$g (9) = 4$
$g (5) = 5$
$g (1) = 6$

此时机器人位于13，起点更新为13，需重新规划从13到16的路径。

更新计算过程

(1) 更新单元格14

单元格14变为障碍物，所有通向14的边代价为∞。
$min_{s' \in Succ(14)} [c(14, s') + g(s')]$

，邻居为10、13、15、18：
- $c (14, 10) = \infty, c (14, 13) = \infty, c (14, 15) = \infty, c (14, 18) = \infty$
- $r h s (14) = \infty$
$g (14) 原为 2 ，但现在设为 \infty ，表示不可达。$

(2) 更新邻居节点

单元格13 (4,1)：

邻居：9、14（堵住）、17
$r h s (13) = min (g (9) + 1, g (17) + 1)$ （14不可达）
- $g (9) = 4, g (17) = \infty （未探索）$
- $r h s (13) = min (4 + 1, \infty) = 5$
$g (13) = 3 ，与 r h s (13) = 5 不一致，需更新。$
$h (13) = ∣4 - 4∣ + ∣1 - 1∣ = 0 （起点为 13 ）$
$键值： [min (3, 5) + 0, min (3, 5)] = [3, 3]$

单元格10 (3,2)：

邻居：6（障碍）、9、11、14（堵住）
$r h s (10) = min (g (9) + 1, g (11) + 1)$ （6和14不可达）
- $g (9) = 4, g (11) = \infty$
- $r h s (10) = min (4 + 1, \infty) = 5$
$g (10) = \infty （未探索），与 r h s (10) 不一致。$
$h (10) = ∣3 - 4∣ + ∣2 - 1∣ = 2$
键值： $[min (\infty, 5) + 2, min (\infty, 5)] = [7, 5]$

单元格15 (4,3)：

邻居：11、14（堵住）、16、19
$r h s (15) = min (g (11) + 1, g (16) + 1, g (19) + 1)$
- $g (11) = \infty, g (16) = 0, g (19) = \infty$
- $r h s (15) = min (\infty, 0 + 1, \infty) = 1$
$g (15) = 1 ，与 r h s (15) = 1 一致，无需更新。$
$h (15) = ∣4 - 4∣ + ∣3 - 1∣ = 2$
键值： $[min (1, 1) + 2, 1] = [3, 1]$

单元格18 (5,2)：

邻居：14（堵住）、17、19
$r h s (18) = min (g (17) + 1, g (19) + 1)$
- $g (17) = \infty, g (19) = \infty$
- $r h s (18) = \infty$
$g (18) = \infty ，与 r h s (18) 一致，无需更新。$

(3) 优先级队列

将不一致的节点加入队列：

13: [3, 3]
10: [7, 5]
15无需加入（已一致）。

路径修复过程

从队列取出键值最小的节点：
- 取出13 ([3, 3])，更新 $g (13) = r h s (13) = 5 。$
扩展13的邻居：
- 9：已知 $g (9) = 4$ ，无需更新。
- 17： $r h s (17) = min (g (13) + 1) = 5 + 1 = 6 ， h (17) = 1 ，键值 [7, 6] 。$
继续扩展：
- 取出17，更新 $g (17) = 6 。$
- 邻居18： $r h s (18) = g (17) + 1 = 7 ，键值 [8, 7] 。$
- 邻居20：不可达。
扩展18：
- $g (18) = 7 ，邻居 19 ： r h s (19) = g (18) + 1 = 8 ，键值 [9, 8] 。$
扩展19：
- $g (19) = 8 ，邻居 15 ： r h s (15) = min (g (19) + 1, g (16) + 1) = min (9, 1) = 1 ，已一致。$
终止：起点13的 $g (13) = r h s (13)$ ，路径收敛。

最终路径：13 → 17 → 18 → 19 → 15 → 16。

完整路径

结合初始部分，完整路径为：

在这里插入图片描述

1 → 5 → 9 → 13 → 17 → 18 → 19 → 15 → 16
总步数：8步

五、伪代码说明

函数 Initialize():
    对于图中所有节点 s：
        g(s) ← ∞            // 初始路径代价未知
        rhs(s) ← ∞          // 估计值未知
    rhs(目标) ← 0            // 目标点设置为路径传播源头
    open_list ← 空堆队列
    将目标点插入 open_list，优先级为 CalculateKey(目标)


函数 CalculateKey(s):
    k1 ← min(g(s), rhs(s)) + 启发函数(start, s) + km
    k2 ← min(g(s), rhs(s))
    返回 [k1, k2]   // 二元组优先级


函数 UpdateVertex(s):
    如果 s ≠ 目标：
        rhs(s) ← min_{s' ∈ 后继(s)} [c(s, s') + g(s')]

    如果 s 已存在于 open_list：
        从 open_list 中删除 s

    如果 g(s) ≠ rhs(s)： // 若节点不一致
        将 s 按 CalculateKey(s) 插入 open_list


函数 ComputeShortestPath():
    当 open_list 最小 key < CalculateKey(start) 或者 rhs(start) ≠ g(start)：
        从 open_list 弹出最小 key 的节点 u

        如果 g(u) > rhs(u)： // 一致性修复（收敛）
            g(u) ← rhs(u)
            对每个 u 的前驱节点 s：
                UpdateVertex(s)

        否则： // 路径无效或被阻断
            g(u) ← ∞
            UpdateVertex(u)
            对每个 u 的前驱节点 s：
                UpdateVertex(s)


函数 Main():
    初始化 km ← 0
    调用 Initialize()
    调用 ComputeShortestPath()

    当 start ≠ goal：
        从 start 的邻居中选择 s，使得 c(start, s) + g(s) 最小
        将 start 移动到 s

        感知环境中是否有障碍变化
        如果有变化：
            km ← km + 启发函数(last_start, current_start)

            对所有受影响的节点 s：
                UpdateVertex(s)

            调用 ComputeShortestPath()

六、Python 简化实现示例

# D* Lite Python实现版，以网格为基础，含详细中文注释 + 可视化运行演示
import heapq
import matplotlib.pyplot as plt
import matplotlib.patches as patches

# 节点实体：包含坐标x,y，路径估计g，期望值rhs，父节点
class Node:
    def __init__(self, x, y):
        self.x, self.y = x, y
        self.g = float('inf')     # 当前最优路径值
        self.rhs = float('inf')   # one-step lookahead路径估计
        self.parent = None        # 可选，用于路径恢复

    def __lt__(self, other):
        return False  # heapq需要，其他部分使用key控制优先级

# 使用曼哈顿距离作为启发函数
def heuristic(a, b):
    return abs(a.x - b.x) + abs(a.y - b.y)

# 计算key，用于在异我队列中排序
# key = [最小g,最小rhs] + h(start, s) + km

def calculate_key(node, start, km):
    min_val = min(node.g, node.rhs)
    return (min_val + heuristic(start, node) + km, min_val)

# 更新节点：不一致（g ≠ rhs）的节点需要加入 open_list
# 如果更新后已符合 g = rhs，则不再加入

def update_vertex(node, graph, goal, open_list, start, km):
    if node != goal:
        node.rhs = min([1 + neighbor.g for neighbor in graph.neighbors(node)])

    for i, (_, n) in enumerate(open_list):
        if n == node:
            open_list.pop(i)
            heapq.heapify(open_list)
            break

    if node.g != node.rhs:
        heapq.heappush(open_list, (calculate_key(node, start, km), node))

# 计算最短路径主循环
def compute_shortest_path(open_list, start, goal, graph, km):
    while open_list:
        top_key, u = open_list[0]
        if top_key >= calculate_key(start, start, km) and start.rhs == start.g:
            break
        heapq.heappop(open_list)
        if u.g > u.rhs:
            u.g = u.rhs
            for s in graph.predecessors(u):
                update_vertex(s, graph, goal, open_list, start, km)
        else:
            u.g = float('inf')
            update_vertex(u, graph, goal, open_list, start, km)
            for s in graph.predecessors(u):
                update_vertex(s, graph, goal, open_list, start, km)

# 网格类
def draw_grid_path(graph, start, goal, path=None):
    fig, ax = plt.subplots()
    ax.set_aspect('equal')
    for x in range(graph.width):
        for y in range(graph.height):
            rect = patches.Rectangle((x, y), 1, 1, linewidth=0.5, edgecolor='gray', facecolor='white')
            ax.add_patch(rect)

    for ox, oy in graph.obstacles:
        rect = patches.Rectangle((ox, oy), 1, 1, facecolor='black')
        ax.add_patch(rect)

    ax.add_patch(patches.Rectangle((start.x, start.y), 1, 1, facecolor='green'))
    ax.add_patch(patches.Rectangle((goal.x, goal.y), 1, 1, facecolor='red'))

    if path:
        for node in path:
            ax.add_patch(patches.Rectangle((node.x, node.y), 1, 1, facecolor='lightblue'))

    plt.xlim(0, graph.width)
    plt.ylim(0, graph.height)
    plt.gca().invert_yaxis()
    plt.grid(True)
    plt.show()

# 获取从起点出发的最短路径（贪心策略）
def extract_path(start, graph):
    path = [start]
    current = start
    while current.rhs != 0:
        neighbors = graph.neighbors(current)
        if not neighbors:
            break
        current = min(neighbors, key=lambda n: 1 + n.g)
        path.append(current)
    return path

# 图结构定义
class GridGraph:
    def __init__(self, width, height, obstacles=set()):
        self.width = width
        self.height = height
        self.obstacles = obstacles
        self.nodes = {
            (x, y): Node(x, y) for x in range(width) for y in range(height)
        }

    def get_node(self, x, y):
        return self.nodes.get((x, y))

    def neighbors(self, node):
        result = []
        for dx, dy in [(-1,0), (1,0), (0,-1), (0,1)]:
            nx, ny = node.x + dx, node.y + dy
            if 0 <= nx < self.width and 0 <= ny < self.height and (nx, ny) not in self.obstacles:
                result.append(self.get_node(nx, ny))
        return result

    def predecessors(self, node):
        return self.neighbors(node)

    def update_obstacle(self, x, y):
        self.obstacles.add((x, y))

    def remove_obstacle(self, x, y):
        self.obstacles.discard((x, y))

# 示例运行函数
def run_demo():
    width, height = 6, 6
    obstacles = {(2,2), (2,3), (3,2)}  # 初始障碍物
    graph = GridGraph(width, height, obstacles)

    start = graph.get_node(0, 0)
    goal = graph.get_node(5, 5)
    km = 0

    goal.rhs = 0
    open_list = [(calculate_key(goal, start, km), goal)]

    update_vertex(start, graph, goal, open_list, start, km)
    compute_shortest_path(open_list, start, goal, graph, km)

    path = extract_path(start, graph)
    draw_grid_path(graph, start, goal, path)

# 执行示例
if __name__ == '__main__':
    run_demo()

七、C++简化实现示例

// D* Lite 算法简化示例 - C++ 版本（基于网格地图）
// 本示例使用四方向网格，障碍可设置为不可达，支持基本路径规划 + 动态障碍更新

#include <iostream>
#include <vector>
#include <queue>
#include <unordered_map>
#include <cmath>
#include <climits>
#include <set>

using namespace std;

struct Node {
    int x, y;
    double g = INFINITY;
    double rhs = INFINITY;
    Node(int x=0, int y=0): x(x), y(y) {}
    bool operator==(const Node& other) const { return x == other.x && y == other.y; }
    bool operator<(const Node& other) const { return tie(x, y) < tie(other.x, other.y); }
};

struct Key {
    double k1, k2;
    bool operator<(const Key& other) const {
        if (k1 == other.k1) return k2 < other.k2;
        return k1 < other.k1;
    }
};

struct CellHash {
    size_t operator()(const Node& n) const {
        return hash<int>()(n.x * 1000 + n.y);
    }
};

class DStarLite {
private:
    int width, height;
    Node start, goal;
    double km = 0;
    set<pair<Key, Node>> open_list;
    unordered_map<Node, double, CellHash> g_vals, rhs_vals;
    vector<vector<int>> grid; // 0=free, 1=obstacle

public:
    DStarLite(int w, int h, Node s, Node g, vector<vector<int>> gridmap)
        : width(w), height(h), start(s), goal(g), grid(gridmap) {
        rhs_vals[goal] = 0;
        g_vals[goal] = INFINITY;
        open_list.insert({calculateKey(goal), goal});
    }

    double heuristic(Node a, Node b) {
        return abs(a.x - b.x) + abs(a.y - b.y);
    }

    Key calculateKey(Node s) {
        double min_g_rhs = min(getG(s), getRHS(s));
        return {min_g_rhs + heuristic(start, s) + km, min_g_rhs};
    }

    double getG(Node s) {
        return g_vals.count(s) ? g_vals[s] : INFINITY;
    }

    double getRHS(Node s) {
        return rhs_vals.count(s) ? rhs_vals[s] : INFINITY;
    }

    vector<Node> getSuccessors(Node s) {
        vector<Node> result;
        for (auto [dx, dy] : vector<pair<int,int>>{{1,0},{-1,0},{0,1},{0,-1}}) {
            int nx = s.x + dx, ny = s.y + dy;
            if (nx >= 0 && nx < width && ny >= 0 && ny < height && grid[ny][nx] == 0)
                result.emplace_back(nx, ny);
        }
        return result;
    }

    void updateVertex(Node u) {
        if (!(u == goal)) {
            double min_rhs = INFINITY;
            for (auto s : getSuccessors(u))
                min_rhs = min(min_rhs, 1 + getG(s));
            rhs_vals[u] = min_rhs;
        }

        open_list.erase({calculateKey(u), u});
        if (getG(u) != getRHS(u))
            open_list.insert({calculateKey(u), u});
    }

    void computeShortestPath() {
        while (!open_list.empty() && 
              (open_list.begin()->first < calculateKey(start) || getRHS(start) != getG(start))) {
            Node u = open_list.begin()->second;
            open_list.erase(open_list.begin());

            if (getG(u) > getRHS(u)) {
                g_vals[u] = getRHS(u);
                for (auto s : getSuccessors(u))
                    updateVertex(s);
            } else {
                g_vals[u] = INFINITY;
                updateVertex(u);
                for (auto s : getSuccessors(u))
                    updateVertex(s);
            }
        }
    }

    void updateObstacle(Node obs) {
        grid[obs.y][obs.x] = 1; // 设置为障碍
        for (auto s : getSuccessors(obs)) updateVertex(s);
        updateVertex(obs);
        km += heuristic(start, obs); // 更新启发偏移量
        computeShortestPath();
    }

    vector<Node> extractPath() {
        vector<Node> path;
        Node current = start;
        path.push_back(current);
        while (!(current == goal)) {
            vector<Node> neighbors = getSuccessors(current);
            Node next;
            double min_cost = INFINITY;
            for (auto s : neighbors) {
                double cost = 1 + getG(s);
                if (cost < min_cost) {
                    min_cost = cost;
                    next = s;
                }
            }
            if (min_cost == INFINITY) break; // no path
            path.push_back(next);
            current = next;
        }
        return path;
    }

    void printPath(const vector<Node>& path) {
        cout << "路径：";
        for (auto n : path)
            cout << "(" << n.x << "," << n.y << ") ";
        cout << endl;
    }
};

int main() {
    vector<vector<int>> grid = {
        {0, 0, 0, 0},
        {0, 1, 0, 1}, // 6 and 8 are obstacles
        {0, 0, 0, 0},
        {0, 0, 0, 0},
        {0, 0, 0, 0}
    };
    Node start(0,0), goal(3,3); // 单元格1到16
    DStarLite dstar(4,5,start,goal,grid);

    // 初次路径规划
    dstar.computeShortestPath();
    auto path = dstar.extractPath();
    dstar.printPath(path);

    // 模拟机器人前进至 (1,3) 后，发现 (1,2)=单元格14 被封堵
    cout << "\n检测到障碍单元格 (1,2)，更新路径..." << endl;
    dstar.updateObstacle(Node(1,2));
    path = dstar.extractPath();
    dstar.printPath(path);

    return 0;
}