算法学习——D*算法

D* 算法是一种高效的动态路径规划算法，特别适合需要实时调整路径的场景。通过合理设置启发式函数、代价函数和重规划频率，可以显著提高路径规划的效率和适应性。

.小墨迹

1722人浏览 · 2025-02-28 14:17:24

.小墨迹 · 2025-02-28 14:17:24 发布

D* 路径规划算法详解

D*（Dynamic A*）算法是一种动态路径规划算法，广泛应用于机器人和自动驾驶领域。它能够在动态和部分未知的环境中高效地重新规划路径，特别适合需要实时调整路径的场景。

1. 算法原理

D* 算法的核心在于动态重规划能力。它通过维护两个值——g(s)（已知代价）和 rhs(s)（估计代价）——来动态调整路径。算法的主要步骤包括：

初始化：
- 将所有状态的 g(s) 和 rhs(s) 初始化为无穷大。
- 将目标状态的 rhs(s) 设置为 0，并将其加入优先队列。
动态规划：
- 使用优先队列管理状态，每次选择优先级最高的状态进行扩展。
- 根据环境变化动态更新 g(s) 和 rhs(s)，重新计算路径。
路径重建：
- 从起点开始，根据 g(s) 值重建路径。

2. 实现方式

D* 算法的实现可以分为以下几个关键步骤：

初始化：

g(s) := infinity for all s in the grid
rhs(s) := infinity for all s in the grid
rhs(goal) := 0
priority_queue := empty
priority_queue.insert(goal, CalculateKey(goal))

更新状态：

def UpdateState(s):
    if g(s) != rhs(s):
        priority_queue.update(s, CalculateKey(s))
    else:
        priority_queue.remove(s)

计算最短路径：

def ComputeShortestPath():
    while priority_queue is not empty and (priority_queue.top_key() < CalculateKey(start) or rhs(start) != g(start)):
        u := priority_queue.pop()
        if g(u) > rhs(u):
            g(u) := rhs(u)
            for all s in Succ(u):
                if s != u:
                    if g(s) > rhs(u) + cost(u, s):
                        g(s) := rhs(u) + cost(u, s)
                        UpdateState(s)
        else:
            g(u) := infinity
            for all s in Pred(u):
                if g(s) > rhs(u) + cost(s, u):
                    g(s) := rhs(u) + cost(s, u)
                    UpdateState(s)

路径规划：

def PlanPath():
    ComputeShortestPath()
    # After computation, the optimal path can be reconstructed from g-values.

3. 路径生成

D* 算法通过动态更新 g(s) 和 rhs(s)，在每次环境变化时重新计算路径。路径生成过程如下：

动态障碍物处理：
- 使用传感器检测动态障碍物，一旦检测到障碍物，立即触发重规划。
实时调整：
- 根据新的障碍物信息，更新 g(s) 和 rhs(s)，重新计算路径。
路径重建：
- 从起点开始，根据 g(s) 值重建路径，确保路径避开障碍物。

4. 约束条件设置

D* 算法的约束条件包括：

障碍物检测：
- 使用传感器（如激光雷达、摄像头）实时检测障碍物。
路径代价：
- 定义代价函数，考虑路径长度、障碍物距离、车辆动力学等。
动态适应性：
- 算法需要能够实时响应环境变化，动态调整路径。

5. 参数调节

D* 算法的性能可以通过以下参数调节：

启发式函数：
- 选择合适的启发式函数（如欧几里得距离或曼哈顿距离）。
代价函数：
- 调整代价函数的权重，优化路径代价。
重规划频率：
- 根据环境动态性调整重规划的频率。

6. 优点与局限性

优点：

动态适应性：能够实时响应环境变化，动态调整路径。
高效重规划：通过动态更新 g(s) 和 rhs(s)，减少不必要的计算。
适用性广泛：适用于动态和部分未知的环境。

局限性：

计算复杂度：在复杂环境中，计算复杂度可能较高。
依赖传感器：算法性能依赖于传感器的准确性和实时性。

7. 总结

D* 算法（特别是 D Lite* 变种）通过动态更新 g(s) 和 rhs(s) 来高效应对环境变化。更新规则和核心逻辑：

1. 关键定义

g(s)：从节点 s 到目标节点的 当前最小代价估计（类似 A* 的 g 值，但动态更新）。
rhs(s)（Right Hand Side）：基于 s 的 父节点 的 g 值计算的 单步回溯代价，满足：
[
rhs(s) = \begin{cases}
0 & \text{如果 } s \text{ 是目标节点} \
\min_{s’ \in \text{Succ}(s)} (c(s, s’) + g(s’)) & \text{否则}
\end{cases}
]
其中 c(s, s') 是从 s 到邻居 s' 的移动代价。

2. 更新规则

(1) 当节点 `s` 的代价需要更新时（例如边代价变化或新障碍物）

更新 rhs(s)：

遍历 s 的所有邻居 s'，计算最小 (c(s, s') + g(s'))。
如果 rhs(s) != g(s)，将 s 标记为 不一致（Inconsistent） 并加入优先队列。

处理不一致节点：

从优先队列中取出最优先的节点 u（按 min(g(u), rhs(u)) + h(u) 排序，h 是启发式函数）。
如果 g(u) > rhs(u)（欠一致）：
[
g(u) = rhs(u) \quad \text{（降低代价，类似“放松”操作）}
]
如果 g(u) < rhs(u)（过一致）：
[
g(u) = \infty \quad \text{（重置代价，需重新计算）}
]
更新 u 的所有邻居的 rhs 值，并将不一致的邻居加入队列。

(2) 初始化和目标节点

目标节点：始终强制 rhs(s_{goal}) = g(s_{goal}) = 0。
其他节点：初始时 g(s) = rhs(s) = ∞。

3. 算法核心逻辑

while queue.has_inconsistent_nodes():
u = queue.pop_min()
if g(u) > rhs(u):# 欠一致
g(u) = rhs(u)
for s in predecessors(u):# 更新所有前驱节点
rhs(s) = min(c(s, s') + g(s') for s' in successors(s))
queue.update(s)
else:# 过一致
g(u) = ∞
for s in predecessors(u):
rhs(s) = min(c(s, s') + g(s') for s' in successors(s))
queue.update(s)

4. 动态环境下的处理

当检测到边代价变化（如新障碍物）：

修改受影响的 c(s, s')。
更新受影响节点的 rhs 值：

对于边 (u, v) 的代价变化，重新计算 rhs(u)。

将受影响节点加入队列，继续传播更新。

5. 直观理解

状态	条件	操作	物理意义
一致	`g(s) == rhs(s)`	无操作	代价已最优
欠一致	`g(s) > rhs(s)`	降低 `g(s)` 至 `rhs(s)`	发现更短路径
过一致	`g(s) < rhs(s)`	重置 `g(s) = ∞`，重新计算	原路径失效（如障碍物出现）