【启发式算法】Dynamic A（D）算法详细介绍（Python）

不去幼儿园

于 2025-06-30 14:42:19 发布

阅读量2.1k

点赞数 88

CC 4.0 BY-SA版权

分类专栏：人工智能（AI） # 启发式算法文章标签：人工智能算法机器学习 python 启发式算法路径规划

本文链接：https://blog.csdn.net/qq_51399582/article/details/149017580

人工智能（AI）同时被 2 个专栏收录

46 篇文章

订阅专栏

启发式算法

10 篇文章

订阅专栏

📢本篇文章是博主人工智能（AI）领域学习时，用于个人学习、研究或者欣赏使用，并基于博主对相关等领域的一些理解而记录的学习摘录和笔记，若有不当和侵权之处，指出后将会立即改正，还望谅解。文章分类在👉启发式算法专栏：

【启发式算法】（10）---《Dynamic A*（D*）算法详细介绍（Python）》

【启发式算法】Dynamic A（D）算法详细介绍（Python）

D*算法（动态A*算法）是一种用于机器人路径规划的算法，特别适合在环境变化的情况下重新计算路径。它的基本思路是动态地、逐步地找到从起点到目标的最短路径，尤其是在障碍物动态变化的情况下。

一、D*算法的背景

传统的A*算法适用于静态地图，也就是说，假设路径规划过程中地图中的障碍物是不会变化的。但在实际情况中，机器人可能会遇到障碍物突然出现或消失，地图也会发生变化。D*算法解决的就是这种动态环境中的路径规划问题。

D*算法最早由 Anthony Stentz 在1994年提出，目的是为了应对动态环境中路径规划的问题。它的起源与自动驾驶、移动机器人、以及无人机的研究密切相关，尤其是在那些需要实时处理地图变化的领域。D*算法是一种 增量式路径规划算法，即当环境发生变化时，它并不会从头开始重新计算整个路径，而是基于之前计算的路径进行增量更新，从而节省计算资源，提高实时响应能力。

D*算法通过不断更新路径，使得它能够在地图发生变化时，调整路径以避免新的障碍物。D有多种变体，其中最经典的是 D Lite*，它是在A*基础上的一种改进方法，更高效且计算量较小。

二、D*算法的工作原理

D*算法的工作原理依赖于动态更新路径的概念，特别是在地图发生变化时通过局部更新来优化路径。虽然它的基础是A*算法，但D*算法在执行过程中使用了一些特定的计算公式和推理步骤来完成动态路径更新。

A*算法基础回顾

首先，A*算法计算路径的基本公式是：

$f(n) = g(n) + h(n)$

$f(n)$ ：当前节点的评估函数，表示从起点到目标的预估总代价。
$g(n)$ ：从起点到当前节点的实际代价（路径的代价）。
$h(n)$ ：从当前节点到目标节点的估计代价（通常是启发式函数，比如曼哈顿距离或欧几里得距离）。

D*算法的基本步骤

D*算法在此基础上做了改进，并引入了几个新的公式来应对动态变化的环境。

1. 初始化：目标节点的值计算

与A*算法相似，D*算法在开始时会对每个节点进行初始化，并计算初始的路径代价。对于每个节点，D*算法会计算启发式值 $h(n)$ ，并为每个节点定义两个重要值：

$g(n)$ ：从起点到当前节点的实际代价。
$rhs(n)$ ：在D*中，rhs是更新后的代价（这是D*独有的）。它表示的是从当前节点到目标节点的代价，并且是在环境变化时动态更新的。

目标节点的初始值设置为：

$rhs(\text{goal}) = 0, \quad g(\text{goal}) = \infty$

2. 更新规则：局部更新

D*算法的核心思想是，当障碍物发生变化时，它不会从头开始重新计算整个路径，而是根据受影响区域局部更新路径。

在每次节点更新时，D*会使用以下两种更新规则来计算新的值：

更新 $g(n)$ 值：

$g(n) = \min_{s \in S_{n}} [c(n, s) + g(s)]$

其中：

$S_{n}$ ：从当前节点 n 的邻居节点集合。

$c(n, s)$ ：从节点 n到邻居节点 s的移动代价（通常是1或欧几里得距离）。

$g(s)$ ：邻居节点 s 到起点的实际代价。

更新 $rhs(n)$ 值：*

$rhs(n) = \min_{s \in S_{n}} [c(n, s) + g(s)]$

这两个公式计算出的是节点的最小代价，并逐步更新路径的估计值。

3. 优先队列更新

D*算法使用一个优先队列来管理待更新的节点。队列中的节点按 $f(n) = g(n) + h(n)$ 排序，并按顺序更新。优先队列的更新方式确保了路径计算的高效性。

4. 反向搜索

D*算法的一个特别之处在于，它是从目标节点开始反向搜索路径，而不是从起点开始。这种反向搜索的方式让它能够只在地图发生变化时局部调整路径。

在反向更新过程中，D*算法会根据更新后的值计算出新的最短路径。当障碍物发生变化时，它会检查受影响的区域，并更新其路径估值：

$g(n) = \min_{s \in S_{n}} [c(n, s) + g(s)]$

这个更新过程会逐渐将影响范围从目标点传播到起点，并逐步调整路径。

5. 增量更新

D*算法的增量更新思想是基于局部路径调整，减少了重新计算整个路径的计算量。当障碍物移动时，D*算法只重新计算那些直接受影响的路径部分，而不重新计算全局路径。

6. 计算最终路径

最终，当路径计算完毕后，D*算法将输出从起点到目标点的最短路径。如果在更新过程中发现障碍物消失或发生了其他变化，D*算法会实时调整路径，保证机器人始终沿着最优路径前进。

D*算法的核心在于基于A*的启发式搜索思想，结合增量更新和反向搜索的技术来实现高效的动态路径规划。通过使用：

g(n)：表示从起点到当前节点的实际代价。
rhs(n)：表示当前节点到目标节点的代价，并在环境发生变化时动态更新。
优先队列：帮助实现最小代价路径的快速选择与更新。

这些公式和更新规则使得D*算法能够高效地处理动态环境中的路径规划问题，减少了计算量并提高了实时反应能力。

D*算法的步骤简述

步骤一：计算初始路径
D算法首先会运行一个类似A的过程，找到从起点到目标点的最短路径。假设地图上有一个障碍物，那么路径规划会绕过这些障碍物，计算出一条最优的路径。
步骤二：检测环境变化
随着时间的推移，障碍物可能会发生变化（比如机器人前进时，突然遇到了新的障碍物）。D*算法会在这种情况下动态更新路径，而不是重新计算整个路径。
步骤三：逐步更新
当障碍物发生变化时，D*算法会从目标点开始，向起点方向逐步更新路径。它只会在受影响的区域重新计算，避免了对未受影响区域的重复计算。
步骤四：优化路径
每次更新后，D*算法会对路径进行优化，确保路径依然是最短的。

三、举例说明

假设你有一个机器人，它需要从起点A走到目标点B，地图上有一些障碍物。初始情况下，D*算法会计算出如下的路径：

起点A → 目标B

但如果在机器人行进过程中，障碍物突然出现在路径上，D*算法会动态地更新路径，确保机器人能够避开障碍物。更新后的路径可能变成：

起点A → 新的路径 → 目标B

通过这种方式，D*算法确保了机器人能够实时适应环境变化，避免了重新计算所有路径的时间浪费。

[Python] D*算法实现

完整的项目代码我已经放入下面链接里面，可以通过下面链接跳转：🔥

人工智能-启发式算法-Dstar算法-动态路径规划

若是下面代码复现困难或者有问题，也欢迎评论区留言。

1.导入库

"""《D*算法项目》
    时间：2025.06.30
    作者：不去幼儿园
"""
import math
from sys import maxsize
import matplotlib.pyplot as plt
from matplotlib import animation
from matplotlib.colors import ListedColormap
import numpy as np

2.状态设置

# 表示地图上每个格子的状态
class State(object):
    def __init__(self, x, y):
        self.x = x  # 行坐标
        self.y = y  # 列坐标
        self.parent = None  # 回溯路径的前驱节点
        self.state = "."  # 状态符号：. 空地，# 障碍，s 起点，e 终点，* 路径
        self.t = "new"  # 标记状态：new/open/close
        self.h = 0  # 启发式代价值
        self.k = 0  # 最小路径估价函数

    def cost(self, state):
        # 如果有障碍返回极大值，否则计算欧几里得距离
        if self.state == "#" or state.state == "#":
            return maxsize
        return math.hypot(self.x - state.x, self.y - state.y)

    def set_state(self, state):
        # 设置格子状态
        if state not in ["s", ".", "#", "e", "*"]:
            return
        self.state = state

3.地图设置

# 表示整个地图，包括网格和障碍
class Map(object):
    def __init__(self, row, col):
        self.row = row
        self.col = col
        self.map = self.init_map()

    def init_map(self):
        # 初始化地图为 State 对象的二维列表
        return [[State(i, j) for j in range(self.col)] for i in range(self.row)]

    def get_neighbers(self, state):
        # 获取一个格子周围8个方向的邻居（包括对角）
        state_list = []
        for i in [-1, 0, 1]:
            for j in [-1, 0, 1]:
                if i == 0 and j == 0:
                    continue
                nx, ny = state.x + i, state.y + j
                if 0 <= nx < self.row and 0 <= ny < self.col:
                    state_list.append(self.map[nx][ny])
        return state_list

    def set_obstacle(self, point_list):
        # 设置地图上的障碍
        for x, y in point_list:
            if 0 <= x < self.row and 0 <= y < self.col:
                self.map[x][y].set_state("#")

4.D*算法


# 实现 D* 路径规划算法
class Dstar(object):
    def __init__(self, maps):
        self.map = maps
        self.open_list = set()  # 存放待处理节点
        self.frames = []  # 存储每帧动画状态
        self.start = None
        self.end = None

    def process_state(self):
        # 核心状态处理函数，根据 D* 三种情况更新路径信息
        x = self.min_state()  # 获取 open list 中代价最小的节点
        if x is None:
            return -1  # 如果没有节点，则返回 -1
        k_old = self.get_kmin()  # 获取当前代价最小的 k 值
        self.remove(x)  # 将节点 x 从 open list 移除
        if k_old < x.h:  # 如果当前 k 小于 x 的启发式代价
            # 处理邻居节点，更新路径和代价
            for y in self.map.get_neighbers(x):
                if y.h <= k_old and x.h > y.h + x.cost(y):
                    x.parent = y
                    x.h = y.h + x.cost(y)
        elif k_old == x.h:  # 如果 k 值相等，更新节点
            for y in self.map.get_neighbers(x):
                if y.t == "new" or (y.parent == x and y.h != x.h + x.cost(y)) or (y.parent != x and y.h > x.h + x.cost(y)):
                    y.parent = x
                    self.insert(y, x.h + x.cost(y))
        else:  # 如果 k 值更大，更新邻居节点
            for y in self.map.get_neighbers(x):
                if y.t == "new" or (y.parent == x and y.h != x.h + x.cost(y)):
                    y.parent = x
                    self.insert(y, x.h + x.cost(y))
                elif y.parent != x and y.h > x.h + x.cost(y):
                    self.insert(y, x.h)
                elif y.parent != x and x.h > y.h + x.cost(y) and y.t == "close" and y.h > k_old:
                    self.insert(y, y.h)
        return self.get_kmin()  # 返回最小的 k 值

    def min_state(self):
        # 获取 open list 中 k 最小的节点
        return min(self.open_list, key=lambda x: x.k) if self.open_list else None

    def get_kmin(self):
        # 获取 open list 中最小的 k 值
        return min((x.k for x in self.open_list), default=-1)

    def insert(self, state, h_new):
        # 将节点插入 open list，并设置其状态
        if state.t == "new":
            state.k = h_new  # 如果是新节点，则直接设置 k 值
        elif state.t == "open":
            state.k = min(state.k, h_new)  # 如果节点已经在 open list 中，取较小的 k 值
        elif state.t == "close":
            state.k = min(state.h, h_new)  # 如果节点已经关闭，则取较小的代价
        state.h = h_new
        state.t = "open"  # 设置为 open 状态
        self.open_list.add(state)  # 将节点添加到 open list

    def remove(self, state):
        # 将节点从 open list 移除
        if state.t == "open":
            state.t = "close"  # 设置为 close 状态
        self.open_list.remove(state)

    def modify_cost(self, x):
        # 修改节点的代价，触发重新插入 open list
        if x.t == "close":
            self.insert(x, x.parent.h + x.cost(x.parent))  # 重新插入并更新代价

    def run(self, start, end):
        path_length = 0  # 初始化路径长度统计
        path_cost = 0    # 初始化路径移动代价
        self.start = start
        self.end = end
        self.open_list.add(end)  # 从终点开始向起点反推
        while True:
            self.process_state()  # 处理当前状态
            if start.t == "close":  # 如果起点已关闭，说明找到路径
                break
        start.set_state("s")  # 设置起点状态
        s = start
        # 回溯路径并设置状态
        while s != end:
            s.set_state("s")
            path_length += 1  # 每步路径加一
            path_cost += s.cost(s.parent) if s.parent else 0  # 累加代价
            self.capture_frame()  # 捕捉当前帧
            s = s.parent
        s.set_state("e")  # 设置终点状态
        self.capture_frame()

        # 模拟新增障碍物触发重规划
        self.map.set_obstacle([(9, i) for i in range(3, 9)])
        self.capture_frame()

        print(f"初始路径长度：{path_length}")
        print(f"总移动代价：{path_cost:.2f}")

        tmp = start
        while tmp != end:
            tmp.set_state("*")  # 标记路径
            self.capture_frame()
            if tmp.parent.state == "#":  # 如果路径节点为障碍，重新规划
                self.modify(tmp)
                continue
            tmp = tmp.parent
        tmp.set_state("e")  # 设置终点状态
        self.capture_frame()

    def modify(self, state):
        # 重新修改路径代价并重规划
        self.modify_cost(state)
        while self.process_state() < state.h:  # 直到代价调整完毕
            pass

    def capture_frame(self):
        # 保存当前地图状态作为动画帧
        self.frames.append(self.get_frame_array())

    def get_frame_array(self):
        # 将状态字符转为颜色编码矩阵
        color_map = {'.': 0, '#': 1, 's': 2, 'e': 3, '*': 4}
        array = [[color_map.get(self.map.map[i][j].state, 0)
                  for j in range(self.map.col)] for i in range(self.map.row)]
        # 强调标记起点终点
        if self.start:
            array[self.start.x][self.start.y] = 2
        if self.end:
            array[self.end.x][self.end.y] = 3
        return array

5.绘图函数


# 绘制动画并保存为 gif
def animate_map(frames, save_path="dstar_path.gif"):
    fig, ax = plt.subplots()
    fig.subplots_adjust(left=0, right=1, bottom=0, top=1)  # 去除边距
    cmap = ListedColormap([
        '#add8e6',  # 浅蓝色 - 背景
        '#ff0000',  # 红色 - 障碍物
        '#0000ff',  # 蓝色 - 起点
        '#ff1493',  # 红色 - 终点（洋红）
        '#32cd32'   # 绿色 - 路径
    ])
    im = ax.imshow(np.array(frames[0]), cmap=cmap, vmin=0, vmax=4)
    ax.axis('off')

    def update(i):
        im.set_array(np.array(frames[i]))
        return [im]

    ani = animation.FuncAnimation(fig, update, frames=len(frames), interval=200, blit=True)
    ani.save(save_path, writer='pillow', fps=5)
    print(f"动画已保存为 GIF：{save_path}")
    plt.close(fig)

6.主函数

# 主函数：构建地图、设置障碍、运行 D*、生成动画
if __name__ == '__main__':
    m = Map(20, 20)
    m.set_obstacle([(4, 3), (4, 4), (4, 5), (4, 6), (5, 3), (6, 3), (7, 3)])  # 设置障碍
    start = m.map[1][2]  # 设置起点
    end = m.map[17][11]  # 设置终点
    dstar = Dstar(m)
    dstar.run(start, end)  # 路径规划
    animate_map(dstar.frames, save_path="dstar_path.gif")  # 动画保存

[Results] 运行结果

[Notice] 注意事项

# 环境配置
Python                  3.11.5
torch                   2.1.0
torchvision             0.16.0
gym                     0.26.2

四、D*算法的应用

D*算法的应用背景主要集中在以下几个领域：

机器人导航与控制：机器人在复杂和动态的环境中需要实时计算最优路径，D*算法能帮助机器人避开障碍物并适应环境的变化。
自动驾驶：在自动驾驶技术中，D*算法被用来实时计算车辆的行驶路径，尤其是在道路环境发生变化（如交通事故、道路封闭）时，车辆能够快速调整行驶路线。
无人机路径规划：无人机在执行任务时，可能会遇到飞行路径上的障碍物或其他动态变化的因素。D*算法可以让无人机在飞行过程中动态地调整飞行路径，确保飞行的安全和效率。

五、优点与挑战

优点	缺点
增量式更新路径，节省计算时间和资源	现复杂，相较于A*算法更难以实现
适应动态环境，能够处理障碍物变化	规模环境变化时表现较差，计算效率下降
反向搜索，避免了从起点开始重新计算路径	局部最优路径问题，可能不返回最优路径
实时性强，适用于实时系统和动态应用场景	内存消耗较大，需要存储更多信息
计算效率高，尤其在障碍物变化较少的情况下	密集障碍环境下效果不理想
节省计算资源，只更新受影响区域的路径	性能瓶颈，在复杂环境中可能遇到性能问题