D* 算法和 D* Lite 算法都是用于动态环境路径规划的增量式搜索算法，适用于机器人、无人机等在未知或变化环境中实时重规划路径。二者核心目标一致，但在实现方式、效率和适用场景上存在关键区别。
一、核心区别对比
| 特性 | D* 算法 | D* Lite 算法 |
|---|---|---|
| 提出时间 | 1994--1995 年（Stentz） | 2002 年（Koenig & Likhachev） |
| 搜索方向 | 从目标点反向搜索到起点 | 同样是从目标点反向搜索 |
| 基础思想 | 基于 Dijkstra 的动态扩展，维护节点代价并局部更新 | 结合 LPA*（Lifelong Planning A*）与 D*，更简洁高效 |
| 数据结构 | 使用 g 和 rhs 类似概念，但实现复杂 |
明确定义 g(s)（实际代价）和 rhs(s)（前瞻代价） |
| 增量更新机制 | 需要显式处理"祖先/后代"关系，传播代价变化 | 利用优先队列自动处理不一致节点，更新更高效 |
| 空间/时间复杂度 | 较高，尤其在大规模地图中 | 更优，适合实时系统和资源受限平台 |
| 对起点移动的处理 | 每次起点变化需大量重算 | 引入 k_m（累计启发值偏移），避免全局重排 |
| 典型应用 | 早期火星探测器（如 NASA 的 Sojourner） | 现代移动机器人、无人机、自动驾驶 |
✅ 简单总结 ：D* Lite 是 D* 的简化、高效、工程友好版本，更适合现代实时系统。
二、算法流程简述
1. D* 算法流程
（参考 [2][4][9]）
- 初始化 ：
- 所有节点
tag = NEW - 目标点
G的h(G) = 0，加入 OPEN 表
- 所有节点
- 反向搜索 ：
- 调用
PROCESS-STATE，从目标点向外扩展，计算各点到目标的最优代价 - 直到当前机器人位置
X被标记为 CLOSED，得到初始路径
- 调用
- 沿路径移动 ：
- 机器人按路径前进
- 检测环境变化 （如新障碍物）：
- 调用
MODIFY-COST更新受影响边的代价 - 将受影响节点重新加入 OPEN
- 再次调用
PROCESS-STATE，直到代价变化传播到当前位置Y
- 调用
- 重复步骤 3--4，直至到达目标
⚠️ D* 的更新依赖显式的"代价传播"和节点状态管理，逻辑较复杂。
2. D* Lite 算法流程
（参考 [1][3][5][7]）
- 初始化 ：
- 设起点
s_start，目标s_goal g(s) = ∞（实际代价），rhs(s) = ∞rhs(s_goal) = 0- 将
s_goal加入优先队列U，键值key = [min(g, rhs) + h, min(g, rhs)]
- 设起点
- 计算初始路径 ：
- 调用
ComputeShortestPath()：- 从
U中取出最小 key 节点 - 若
g ≠ rhs，则更新该节点（UpdateVertex） - 重复直到队列空或当前起点一致
- 从
- 调用
- 沿路径移动 ：
- 机器人向目标移动
- 感知环境变化 （如新增障碍）：
- 更新受影响节点的
rhs值 - 若
rhs ≠ g，将节点加入/更新到U - 更新
k_m += h(old_start, new_start)（处理起点移动） - 重新调用
ComputeShortestPath()
- 更新受影响节点的
- 循环步骤 3--4，直到到达目标
✅ D* Lite 通过
g/rhs不一致性驱动更新，配合优先队列，自动聚焦于关键区域，无需手动传播。
三、关键机制差异说明
-
局部一致性（Local Consistency）：
- 当
g(s) == rhs(s)，节点一致；否则需处理。 - D* Lite 利用此机制仅更新必要节点，大幅提升效率。
- 当
-
启发函数使用：
- 两者都用启发函数
h(s)（如欧氏距离），但 D* Lite 的 key 设计更鲁棒。
- 两者都用启发函数
-
工程实现难度：
- D* 实现复杂，易出错；
- D* Lite 伪代码清晰，广泛用于 ROS、Simulink 等平台（见 [5]）。
四、适用场景建议
- 选 D*：历史系统兼容、理论研究、特定航天任务（已验证）
- 选 D* Lite ：绝大多数现代机器人、无人机、自动驾驶、游戏 AI 等需要高效、实时、可扩展的场景
python实现D（D-Star）路径规划算法。D算法是动态A*算法的改进，适用于动态环境中的路径规划。
- 基础D*算法实现
python
import heapq
import math
from typing import Dict, List, Tuple, Set, Optional
import numpy as np
class DStarNode:
"""D*算法节点类"""
def __init__(self, x: int, y: int):
self.x = x
self.y = y
self.g = float('inf') # 从起点到当前节点的代价
self.rhs = float('inf') # 右侧值，用于D*算法
self.parent = None # 父节点
self.key = (float('inf'), float('inf')) # 优先队列键值
def __lt__(self, other):
return self.key < other.key
def __eq__(self, other):
return self.x == other.x and self.y == other.y
def __hash__(self):
return hash((self.x, self.y))
class DStar:
"""D*路径规划算法实现"""
def __init__(self, grid: np.ndarray):
"""
初始化D*算法
Args:
grid: 网格地图，0表示可通行，1表示障碍物
"""
self.grid = grid
self.rows, self.cols = grid.shape
self.nodes: Dict[Tuple[int, int], DStarNode] = {}
self.U = [] # 优先队列
self.km = 0 # 路径代价增量
self.start = None
self.goal = None
# 8个方向的移动成本（包括对角线）
self.directions = [
(-1, -1), (-1, 0), (-1, 1),
(0, -1), (0, 1),
(1, -1), (1, 0), (1, 1)
]
# 对角线移动成本为√2≈1.414，直线移动成本为1
self.direction_cost = [
1.414, 1, 1.414,
1, 1,
1.414, 1, 1.414
]
def get_node(self, x: int, y: int) -> DStarNode:
"""获取或创建节点"""
if (x, y) not in self.nodes:
self.nodes[(x, y)] = DStarNode(x, y)
return self.nodes[(x, y)]
def calculate_key(self, node: DStarNode) -> Tuple[float, float]:
"""计算节点的键值"""
return (
min(node.g, node.rhs) + self.heuristic(node, self.goal) + self.km,
min(node.g, node.rhs)
)
def heuristic(self, node1: DStarNode, node2: DStarNode) -> float:
"""启发式函数（欧几里得距离）"""
return math.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2)
def cost(self, node1: DStarNode, node2: DStarNode) -> float:
"""计算两个节点之间的移动代价"""
dx = abs(node1.x - node2.x)
dy = abs(node1.y - node2.y)
# 检查是否为有效移动
if not (0 <= node2.x < self.rows and 0 <= node2.y < self.cols):
return float('inf')
# 检查目标节点是否为障碍物
if self.grid[node2.x, node2.y] == 1:
return float('inf')
# 检查移动过程中是否穿过障碍物（对于对角线移动）
if dx == 1 and dy == 1:
# 检查两个相邻的直角位置
if (self.grid[node1.x, node2.y] == 1 and
self.grid[node2.x, node1.y] == 1):
return float('inf')
# 返回移动代价
if dx == 1 and dy == 1:
return 1.414
else:
return 1.0
def get_neighbors(self, node: DStarNode) -> List[DStarNode]:
"""获取节点的所有邻居节点"""
neighbors = []
for i, (dx, dy) in enumerate(self.directions):
nx, ny = node.x + dx, node.y + dy
if 0 <= nx < self.rows and 0 <= ny < self.cols:
neighbors.append(self.get_node(nx, ny))
return neighbors
def update_vertex(self, u: DStarNode):
"""更新顶点状态"""
if u != self.goal:
# 找到最小rhs值
min_rhs = float('inf')
best_parent = None
for v in self.get_neighbors(u):
cost_uv = self.cost(u, v)
if cost_uv + v.g < min_rhs:
min_rhs = cost_uv + v.g
best_parent = v
u.rhs = min_rhs
u.parent = best_parent
# 如果节点在优先队列中，移除它
if u in [item[1] for item in self.U]:
self.U = [item for item in self.U if item[1] != u]
heapq.heapify(self.U)
# 如果g不等于rhs，将节点加入优先队列
if u.g != u.rhs:
u.key = self.calculate_key(u)
heapq.heappush(self.U, (u.key, u))
def compute_shortest_path(self):
"""计算最短路径"""
while self.U and (self.U[0][0] < self.calculate_key(self.start) or
self.start.rhs != self.start.g):
_, u = heapq.heappop(self.U)
if u.g > u.rhs:
u.g = u.rhs
for v in self.get_neighbors(u):
self.update_vertex(v)
else:
u.g = float('inf')
self.update_vertex(u)
for v in self.get_neighbors(u):
self.update_vertex(v)
def plan(self, start: Tuple[int, int], goal: Tuple[int, int]) -> Optional[List[Tuple[int, int]]]:
"""
规划路径
Args:
start: 起点坐标 (x, y)
goal: 终点坐标 (x, y)
Returns:
路径列表，包含从起点到终点的坐标序列
"""
# 初始化起点和终点
self.start = self.get_node(*start)
self.goal = self.get_node(*goal)
# 重置所有节点
for node in self.nodes.values():
node.g = float('inf')
node.rhs = float('inf')
# 初始化目标节点
self.goal.rhs = 0
self.goal.key = self.calculate_key(self.goal)
heapq.heappush(self.U, (self.goal.key, self.goal))
# 计算初始路径
self.compute_shortest_path()
# 如果找不到路径，返回None
if self.start.rhs == float('inf'):
return None
# 重建路径
path = []
current = self.start
while current != self.goal:
path.append((current.x, current.y))
if current.parent is None:
return None
current = current.parent
path.append((self.goal.x, self.goal.y))
return path
def replan(self, changed_cells: List[Tuple[int, int, float]]):
"""
重新规划路径（处理环境变化）
Args:
changed_cells: 变化的单元格列表，每个元素为 (x, y, new_cost)
new_cost: 新的移动代价（float('inf')表示障碍物）
"""
# 更新km值
self.km += self.heuristic(self.start, self.get_node(*changed_cells[0][:2]))
# 更新变化的单元格
for x, y, new_cost in changed_cells:
node = self.get_node(x, y)
# 更新网格（这里简化处理，实际可能需要更复杂的代价更新）
if new_cost == float('inf'):
self.grid[x, y] = 1 # 变为障碍物
else:
self.grid[x, y] = 0 # 变为可通行
self.update_vertex(node)
# 重新计算路径
self.compute_shortest_path()- 增强版D* Lite算法实现
python
class DStarLite(DStar):
"""D* Lite算法（D*的优化版本）"""
def __init__(self, grid: np.ndarray):
super().__init__(grid)
self.s_last = None # 记录上一次的位置
def initialize(self, start: Tuple[int, int], goal: Tuple[int, int]):
"""初始化D* Lite"""
self.start = self.get_node(*start)
self.goal = self.get_node(*goal)
self.s_last = self.start
# 初始化所有节点
for node in self.nodes.values():
node.g = float('inf')
node.rhs = float('inf')
# 初始化优先队列
self.U = []
self.km = 0
# 设置目标节点
self.goal.rhs = 0
self.goal.key = self.calculate_key(self.goal)
heapq.heappush(self.U, (self.goal.key, self.goal))
def update_rhs(self, u: DStarNode):
"""更新节点的rhs值"""
if u != self.goal:
min_rhs = float('inf')
best_parent = None
for v in self.get_neighbors(u):
cost_uv = self.cost(u, v)
if cost_uv + v.g < min_rhs:
min_rhs = cost_uv + v.g
best_parent = v
u.rhs = min_rhs
u.parent = best_parent
else:
u.rhs = 0
u.parent = None
def compute_shortest_path(self):
"""计算最短路径（D* Lite版本）"""
while self.U and (self.U[0][0] < self.calculate_key(self.start) or
self.start.rhs > self.start.g):
k_old, u = heapq.heappop(self.U)
k_new = self.calculate_key(u)
if k_old < k_new:
u.key = k_new
heapq.heappush(self.U, (k_new, u))
elif u.g > u.rhs:
u.g = u.rhs
for v in self.get_neighbors(u):
if v != self.goal:
self.update_rhs(v)
self.update_vertex(v)
else:
u.g = float('inf')
for v in self.get_neighbors(u) + [u]:
if v != self.goal:
self.update_rhs(v)
self.update_vertex(v)
def move_and_replan(self, start: Tuple[int, int],
changed_cells: List[Tuple[int, int, float]] = None) -> List[Tuple[int, int]]:
"""
移动并重新规划
Args:
start: 新的起点位置
changed_cells: 环境变化信息
Returns:
新的路径
"""
# 更新起点
old_start = self.start
self.start = self.get_node(*start)
# 更新km
self.km += self.heuristic(old_start, self.start)
# 处理环境变化
if changed_cells:
for x, y, new_cost in changed_cells:
node = self.get_node(x, y)
# 更新网格
if new_cost == float('inf'):
self.grid[x, y] = 1
else:
self.grid[x, y] = 0
# 更新受影响的节点
self.update_rhs(node)
self.update_vertex(node)
# 重新计算路径
self.compute_shortest_path()
# 获取下一步
if self.start.parent:
return [(self.start.x, self.start.y),
(self.start.parent.x, self.start.parent.y)]
return [(self.start.x, self.start.y)]- 可视化工具
python
import matplotlib.pyplot as plt
import matplotlib.animation as animation
class DStarVisualizer:
"""D*算法可视化类"""
def __init__(self, grid_size: Tuple[int, int] = (20, 20),
obstacle_density: float = 0.2):
self.grid = np.zeros(grid_size)
self.rows, self.cols = grid_size
# 随机生成障碍物
num_obstacles = int(grid_size[0] * grid_size[1] * obstacle_density)
for _ in range(num_obstacles):
x = np.random.randint(0, grid_size[0])
y = np.random.randint(0, grid_size[1])
self.grid[x, y] = 1
self.dstar = DStarLite(self.grid)
def visualize_path(self, start: Tuple[int, int], goal: Tuple[int, int],
dynamic_changes: List[List[Tuple[int, int, float]]] = None):
"""可视化路径规划过程"""
fig, ax = plt.subplots(figsize=(10, 10))
# 初始化路径规划
self.dstar.initialize(start, goal)
path = self.dstar.plan(start, goal)
# 绘制初始地图
ax.imshow(self.grid, cmap='Greys', origin='upper')
ax.plot(start[1], start[0], 'go', markersize=15, label='Start')
ax.plot(goal[1], goal[0], 'ro', markersize=15, label='Goal')
if path:
path_y, path_x = zip(*path)
ax.plot(path_x, path_y, 'b-', linewidth=2, label='Path')
ax.legend()
ax.set_title('D* Lite Path Planning')
plt.show()
# 动态变化演示
if dynamic_changes:
self.visualize_dynamic_changes(start, goal, dynamic_changes)
def visualize_dynamic_changes(self, start: Tuple[int, int], goal: Tuple[int, int],
changes_list: List[List[Tuple[int, int, float]]]):
"""可视化动态环境变化"""
fig, ax = plt.subplots(figsize=(10, 10))
current_pos = list(start)
paths = []
for i, changes in enumerate(changes_list):
ax.clear()
# 绘制地图
ax.imshow(self.dstar.grid, cmap='Greys', origin='upper')
ax.plot(goal[1], goal[0], 'ro', markersize=15)
ax.plot(current_pos[1], current_pos[0], 'go', markersize=15)
# 移动并重新规划
next_step = self.dstar.move_and_replan(tuple(current_pos), changes)
if len(next_step) > 1:
current_pos = list(next_step[1])
# 绘制路径
if hasattr(self.dstar.start, 'parent'):
path = []
current = self.dstar.start
while current and current != self.dstar.goal:
path.append((current.x, current.y))
current = current.parent
if current == self.dstar.goal:
path.append((current.x, current.y))
if path:
path_y, path_x = zip(*path)
ax.plot(path_x, path_y, 'b-', linewidth=2)
ax.set_title(f'Dynamic Replanning - Step {i+1}')
plt.pause(1)
plt.show()- 使用示例
python
def main():
# 创建地图
grid_size = (15, 15)
grid = np.zeros(grid_size)
# 添加一些障碍物
obstacles = [(5, 5), (5, 6), (5, 7), (6, 5), (7, 5),
(10, 10), (10, 11), (11, 10), (11, 11)]
for obs in obstacles:
grid[obs] = 1
# 创建D* Lite规划器
planner = DStarLite(grid)
# 设置起点和终点
start = (0, 0)
goal = (14, 14)
# 初始规划
print("Initial planning...")
path = planner.plan(start, goal)
if path:
print(f"Path found with {len(path)} steps:")
for i, (x, y) in enumerate(path):
print(f" Step {i}: ({x}, {y})")
# 模拟动态环境变化
print("\nSimulating dynamic obstacles...")
# 添加新障碍物
new_obstacles = [(7, 7), (8, 8)]
changes = []
for obs in new_obstacles:
grid[obs] = 1
changes.append((obs[0], obs[1], float('inf')))
# 重新规划
current_pos = path[1] if len(path) > 1 else start
new_path_segment = planner.move_and_replan(current_pos, changes)
print(f"Replanned path segment: {new_path_segment}")
# 可视化
visualizer = DStarVisualizer((15, 15), 0.15)
visualizer.visualize_path(start, goal)
else:
print("No path found!")
if __name__ == "__main__":
main()- 算法特点说明
D*算法的主要特点：
- 增量式规划：可以在环境变化时高效重新规划
- 反向搜索：从目标点向起点搜索
- 适应性：适用于动态变化的未知环境
- 高效性：只更新受影响的部分，避免完全重新规划
关键参数调节：
· 启发式函数：可以替换为曼哈顿距离或其他启发式
· 移动代价：可以根据地形调整
· 更新策略：可以根据具体应用调整重新规划的频率
这个实现包含了完整的D*算法框架，支持动态环境下的路径规划，并提供了可视化工具。您可以根据具体需求进行调整和优化。