【啟發式算法】Dynamic A*（D*）算法詳細介紹（Python）

? ? ? ? 📢本篇文章是博主人工智能（AI）領域學習時，用于個人學習、研究或者欣賞使用，并基于博主對相關等領域的一些理解而記錄的學習摘錄和筆記，若有不當和侵權之處，指出后將會立即改正，還望諒解。文章分類在👉啟發式算法專欄：

? ? ? ?【啟發式算法】（10）---《Dynamic A*（D*）算法詳細介紹（Python）》

【啟發式算法】Dynamic A（D）算法詳細介紹（Python）

一、D*算法的背景

二、D*算法的工作原理

?A*算法基礎回顧

D*算法的基本步驟

1. 初始化：目標節點的值計算

2. 更新規則：局部更新

3. 優先隊列更新

4. 反向搜索

5. 增量更新

6. 計算最終路徑

D*算法的步驟簡述

三、舉例說明

[Python] D*算法實現

[Results] 運行結果

[Notice]? 注意事項

四、D*算法的應用

五、優點與挑戰?

六、總結

????????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/closeself.h = 0  # 啟發式代價值self.k = 0  # 最小路徑估價函數def cost(self, state):# 如果有障礙返回極大值，否則計算歐幾里得距離if self.state == "#" or state.state == "#":return maxsizereturn math.hypot(self.x - state.x, self.y - state.y)def set_state(self, state):# 設置格子狀態if state not in ["s", ".", "#", "e", "*"]:returnself.state = state

?3.地圖設置

# 表示整個地圖，包括網格和障礙
class Map(object):def __init__(self, row, col):self.row = rowself.col = colself.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:continuenx, ny = state.x + i, state.y + jif 0 <= nx < self.row and 0 <= ny < self.col:state_list.append(self.map[nx][ny])return state_listdef 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 = mapsself.open_list = set()  # 存放待處理節點self.frames = []  # 存儲每幀動畫狀態self.start = Noneself.end = Nonedef process_state(self):# 核心狀態處理函數，根據 D* 三種情況更新路徑信息x = self.min_state()  # 獲取 open list 中代價最小的節點if x is None:return -1  # 如果沒有節點，則返回 -1k_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 = yx.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 = xself.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 = xself.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 Nonedef 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_newstate.t = "open"  # 設置為 open 狀態self.open_list.add(state)  # 將節點添加到 open listdef remove(self, state):# 將節點從 open list 移除if state.t == "open":state.t = "close"  # 設置為 close 狀態self.open_list.remove(state)def modify_cost(self, x):# 修改節點的代價，觸發重新插入 open listif 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 = startself.end = endself.open_list.add(end)  # 從終點開始向起點反推while True:self.process_state()  # 處理當前狀態if start.t == "close":  # 如果起點已關閉，說明找到路徑breakstart.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.parents.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 = startwhile tmp != end:tmp.set_state("*")  # 標記路徑self.capture_frame()if tmp.parent.state == "#":  # 如果路徑節點為障礙，重新規劃self.modify(tmp)continuetmp = tmp.parenttmp.set_state("e")  # 設置終點狀態self.capture_frame()def modify(self, state):# 重新修改路徑代價并重規劃self.modify_cost(state)while self.process_state() < state.h:  # 直到代價調整完畢passdef 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] = 2if self.end:array[self.end.x][self.end.y] = 3return 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*算法更難以實現
適應動態環境，能夠處理障礙物變化	規模環境變化時表現較差，計算效率下降
反向搜索，避免了從起點開始重新計算路徑	局部最優路徑問題，可能不返回最優路徑
實時性強，適用于實時系統和動態應用場景	內存消耗較大，需要存儲更多信息
計算效率高，尤其在障礙物變化較少的情況下	密集障礙環境下效果不理想
節省計算資源，只更新受影響區域的路徑	性能瓶頸，在復雜環境中可能遇到性能問題