D*路徑搜索算法原理解析及Python實現

1.D*算法簡述

D*是以下三種相關增量搜索算法之一:

• 最初的D* (Anthony Stentz的)是一種知情的增量搜索算法。
• Focused D是Anthony Stentz設計的一種增量啓發式搜索算法，它結合了A[3]和原始D的思想。Focused D源於對原始D*的進一步開發。
• D* Lite[4]是Sven Koenig和Maxim Likhachev基於LPA的增量啓發式搜索算法，是結合A思想和動態SWSF-FP的增量啓發式搜索算法。
  這三種搜索算法都解決了相同的基於假設的路徑規劃問題，包括利用空閒空間假設進行規劃，其中機器人必須在未知地形中導航到給定的目標座標[7]。它對地形的未知部分(例如:它不包含障礙物)進行假設，並在這些假設下找到從當前座標到目標座標的最短路徑。然後機器人沿着這條路走。當它觀察到新的地圖信息(如以前未知的障礙)時，它將這些信息添加到地圖中，並在必要時重新規劃從當前座標到給定目標座標的新的最短路徑。它重複這個過程，直到達到目標座標或確定無法達到目標座標。當穿越未知地形時，可能會經常發現新的障礙物，所以這種重新規劃需要快速。增量式(啓發式)搜索算法利用以往問題的經驗加快對當前問題的搜索，從而加快對相似搜索問題序列的搜索。假設目標座標不變，三種搜索算法都比重複的A搜索更有效。
  D及其變體已廣泛應用於移動機器人和自主車輛導航。當前的系統通常基於D* Lite，而不是最初的D或Focused D。事實上，甚至Stentz的實驗室在一些實現中也使用D* Lite而不是D*。這些導航系統包括在“機遇號”和“勇氣號”火星漫遊者上測試的原型系統，以及在美國國防部高級研究計劃局城市挑戰賽中獲勝的導航系統。
  最初的D是Anthony Stentz在1994年提出的。名稱D來自術語“Dynamic A*”，因爲該算法的行爲類似於A*，只是在算法運行時圓弧成本可能發生變化。

2.操作

D_star的基本操作概述如下。
與Dijkstra算法和A*類似，D *維護要評估的節點列表，稱爲“OPEN list”。節點被標記爲具有以下幾種狀態之一:

• NEW：意味着它從未被列入OPEN list
• OPEN：意味着它當前在OPEN list中
• CLOSED：意味着它不在OPEN list中
• RAISE：意味着它的成本比上次OPEN list時要高
• LOWER：意味着它的成本比上次OPEN list時要低

2.1擴張

該算法通過迭代地從OPEN list中選擇一個節點並對其求值來工作。然後，它將節點的變化傳播到所有相鄰節點，並將它們放到OPEN list中。這種傳播過程稱爲“擴張”。與從始至終遵循路徑的canonical A_star不同，D*從目標節點開始向後搜索。每個擴張節點都有一個反向指針，它指向指向目標的下一個節點，每個節點都知道目標的確切成本。當開始節點是下一個要展開的節點時，算法就完成了，只需遵循反向指針就可以找到目標的路徑。

擴張過程。結束節點(黃色)位於點的頂部行中間，開始節點位於底部行中間。紅色表示障礙;黑色/藍色表示擴張節點(亮度表示成本)。綠色表示正在展開的節點。

完成擴張。路徑以青色表示。

2.2障礙處理

當在指定的路徑上檢測到障礙物時，所有受影響的點將再次被放到OPEN列表中，這次標記爲RAISE。然而，在一個RAISED的節點增加成本之前，算法會檢查它的鄰居，並檢查它是否可以降低節點的成本。如果沒有，則提升狀態傳播到所有節點的後代，即具有反向指針的節點。然後評估這些節點，並且傳遞RAISE狀態，形成波。 當RAISED節點可以減少時，它的反向指針（backpointer）會更新，並將LOWER狀態傳遞給它的鄰居。這些RAISE和LOWER的狀態波是D*的核心。
到這個時候，一系列其他的點就不會被波浪“碰觸”了。因此，該算法只適用於受成本變化影響的點。

添加了一個障礙(紅色)和標記爲RAISE(黃色)的節點。

正在擴張中。 黃色表示標記爲RAISE的節點，綠色表示標記爲LOWER的節點。

2.3 發生死鎖

這一次，不能如此優雅地繞過死鎖。沒有一個點可以通過鄰居找到一條新的路線到達目的地。因此，他們繼續傳播他們的成本增加。只有在通道外才能找到點，這些點可以通過可行的路線到達目的地。這就是兩個較低的波是如何發展的，它們擴張成具有新路線信息的不可到達的標記點。

通道被其他障礙物阻擋(紅色)

正在膨脹(黃色波浪上升，綠色波浪下降)

新路徑找到(青色)

3.僞代碼

while(!openList.isEmpty()) {
  point = openList.getFirst();
  expand(point);
}

3.1擴張

  void expand(currentPoint) {
   boolean isRaise = isRaise(currentPoint);
   double cost;
   foreach(neighbor in currentPoint.getNeighbors()) {
    if(isRaise) {
     if(neighbor.nextPoint == currentPoint) {
      neighbor.setNextPointAndUpdateCost(currentPoint);
      openList.add(neighbor);
     } else {
      cost = neighbor.calculateCostVia(currentPoint);
      if(cost < neighbor.getCost()) {
       currentPoint.setMinimumCostToCurrentCost();
       openList.add(currentPoint);
      }
     }
    } else {
      cost = neighbor.calculateCostVia(currentPoint);
      if(cost < neighbor.getCost()) {
       neighbor.setNextPointAndUpdateCost(currentPoint);
       openList.add(neighbor);
      }
    }
   }
  }

3.2Raise檢查

boolean isRaise(point) {
 double cost;
 if(point.getCurrentCost() > point.getMinimumCost()) {
  foreach(neighbor in point.getNeighbors()) {
   cost = point.calculateCostVia(neighbor);
   if(cost < point.getCurrentCost()) {
    point.setNextPointAndUpdateCost(neighbor);
   }
  }
 }
 return point.getCurrentCost() > point.getMinimumCost();
}

4.變體

Focused D*

顧名思義，Focused D是D的一個擴展，它使用啓發式的方法來聚焦（focus）RAISE、LOWER對機器人的傳播。這樣，只更新重要的狀態，就像A*只計算某些節點的成本一樣。

D* Lite

D* Lite不是基於原始的D或聚焦的D，而是實現了相同的行爲。它更容易理解，而且可以用更少的代碼行實現，因此名爲“D* Lite”。在性能方面，它和Focused D一樣好，甚至更好。D Lite基於Lifelong Planning A*，這是Koenig和Likhachev在幾年前提出的。

5.最小成本與當前成本之比

對於D*，區分當前成本和最低成本是很重要的。前者只在收集時重要，而後者非常重要，因爲它對OpenList進行了排序。返回最小成本的函數總是當前點的最低成本，因爲它是OpenList的第一個條目。

6.經典論文算法介紹

“D_star算法”的名稱源自 Dynamic A Star,最初由Anthony Stentz於“Optimal and Efficient Path Planning for Partially-Known Environments”中介紹。它是一種啓發式的路徑搜索算法，適合面對周圍環境未知或者周圍環境存在動態變化的場景。
同A_star算法類似，D-star通過一個維護一個優先隊列（OpenList）來對場景中的路徑節點進行搜索，所不同的是，D*不是由起始點開始搜索，而是以目標點爲起始，通過將目標點置於Openlist中來開始搜索，直到機器人當前位置節點由隊列中出隊爲止（當然如果中間某節點狀態有動態改變，需要重新尋路，所以纔是一個動態尋路算法）。

6.1符號表示

主要介紹一下論文中用到的一些符號及其含義。
論文中將地圖中的路徑點用State表示，每一個State包含如下信息：

• Backpointer: 指向前一個state的指針，指向的state爲當前狀態的父輩，當前state稱爲指針指向state的後代，目標state無Backpointer。（路徑搜索完畢後，通過機器人所在的state，通過backpointer即可一步步地移動到目標Goal state，GoalState以後用 G表示），b（X）=Y表示X的父輩爲Y。
• Tag：表示當前state的狀態，有 New、Open、Closed三種狀態，New表示該State從未被置於Openlist中，Open表示該State正位於OpenList中，Closed表示已不再位於Openlist中。
• H（X）：代價函數估計，表示當前State到G的開銷估計。
• K（X）：Key Function，該值是優先隊列Openlist中的排序依據，K值最小的State位於隊列頭，對於處於OpenList上的State X，K（X）表示從X被置於Openlist後，X到G的最小代價H（X），可以簡單理解爲K（X）將位於Openlist的State X劃分爲兩種不同的狀態，一種狀態爲Raise（如果K（X）<H（X）），用來傳遞路徑開銷的增加（例如某兩點之間開銷的增加，會導致與之相關的節點到目標的路徑開銷隨之增加）；另一種狀態爲 Lower（如果K（X）=H（X）），用來傳遞路徑開銷的減少（例如某兩點之間開銷的減少，或者某一新的節點被加入到Openlist中，可能導致與之相關的節點到目標的路徑開銷隨之減少）。
• $k_{min}$：表示所有位於Openlist上的state的最小K值。
• C（X,Y）：表示X與Y之間的路徑開銷。
• Openlist 是依據K值由小到大進行排序的優先隊列。

6.2算法描述

搜索的關鍵是state的傳遞過程，即由G向機器人所在位置進行搜索的過程，這種傳遞過程是通過不斷地從當前OpenList中取出K值最小的State來實現的，每當一個State由Openlist中移出時，它會將開銷傳遞給它的鄰域state，這些鄰域state會被置於Openlist中，持續進行該循環，直到機器人所在State的狀態爲 Closed ，或者Openlist爲空（表示不存在到G的路徑）。
算法最主要的是兩個函數，Process-State 和 Modify-Cost，前者用於計算到目標G的最優路徑，後者用於改變兩個state之間的開銷C（X,Y）並將受影響的state置於Openlist中。
算法的主要流程，在初始時，所有state的t（Tag）被設置爲New，H（G）被設置爲0，G被放置於OpenList，然後Process-State函數被不斷執行，直到機器人所處state X由openlist中出隊，然後可以通過機器人的當前state按backpointer指向目標G。當移動過程中發現新探測到的障礙時，Modify-Cost函數立刻被調用，來更正C（X,Y）中的路徑開銷並將受影響的state重新置於openlist中。
令Y表示robot發現障礙時所在的state，通過不斷調用Process-State直到kmin≥H（Y），這時表示路徑開銷的更改已經傳播到了Y，此時，新的路徑構建完成。
論文中的僞代碼如下：

簡要解釋爲：

• L1-L3表示擁有最低K值的X由openlist中移出，如果X爲Lower，那麼它的路徑代價爲最優的。
• 在L8-L13，X爲Lower狀態，X的所有鄰接state都被檢測是否其路徑代價可以更低，狀態爲New的鄰接state被賦予初始路徑開銷值，並且開銷的變動被傳播給每一個backpointer指向X的鄰接state Y（不管這個新的開銷比原開銷大或者小），也就是說只要你指向了X，那麼X的路徑開銷變動時，你的路徑代價必須隨之改變。這裏可能存在由於X路徑開銷變動過大，Y可以通過非X的其他state到達目標且路徑開銷更小的情況，這點在L8-13中並沒有處理，而是放在後續針對Y的process-state函數中，在對Y進行處理時，會將其backpointer指向周圍路徑開銷最小的state。如果X的鄰接State狀態爲New時，應將其鄰接state的backpointer指向X。所有路徑開銷有所變動的state都被置於Openlist中進行處理，從而將變動傳播給鄰接的state。
• 在L4-L7中X爲Raise，它的路徑開銷H可能不是最優的，通過其鄰居state中已經處於最優開銷（即h（Y）≤kold）的節點來優化X的路徑開銷，如果存在更短的路徑，則將X的backpointer指向其neighbor。
• 在L15-L18中，開銷變動傳播到狀態爲New的鄰居state。如果X可以使一個backpointer並不指向X的鄰居state的路徑開銷最小，即Y通過X到目標G的距離更短，但是此時Y的backpointer並不指向X，針對這種情況，可以將X重新置於Openlist中進而優化Y。
• 在L23-25中，如果X可以通過一個狀態爲closed的並不是最理想的鄰居stateY來減小路徑開銷，那麼將Y重新置於Openlist中。

在modify-cost中，更新C（X,Y）並將X重新置於Openlist中，當X通過process-state進行傳播時，會對Y進行開銷計算，$h(Y)=h(X)+c(X,Y)$。

7.D*算法的另一種理解

以下來自於參考資料[2]:

1. 先用Dijstra算法從目標節點G向起始節點搜索。儲存路網中目標點到各個節點的最短路和該位置到目標點的實際值h,k（k爲所有變化h之中最小的值,當前爲k=h。原OPEN和CLOSE中節點信息保存。
2. 機器人沿最短路開始移動，在移動的下一節點沒有變化時，無需計算，利用上一步Dijstra計算出的最短路信息從出發點向後追述即可，當在Y點探測到下一節點X狀態發生改變，如堵塞。機器人首先調整自己在當前位置Y到目標點G的實際值h(Y)，h(Y)=X到Y的新權值c(X,Y)+X的原實際值h(X).X爲下一節點(到目標點方向Y->X->G），Y是當前點。k值取h值變化前後的最小。機器人沿最短路開始移動，在移動的下一節點沒有變化時，無需計算，利用上一步Dijstra計算出的最短路信息從出發點向後追述即可，當在Y點探測到下一節點X狀態發生改變，如堵塞。機器人首先調整自己在當前位置Y到目標點G的實際值h(Y)，h(Y)=X到Y的新權值c(X,Y)+X的原實際值h(X).X爲下一節點(到目標點方向Y->X->G），Y是當前點。k值取h值變化前後的最小。
3. 用A_star或其它算法計算，這裏假設用A_star算法,遍歷Y的子節點，點放入CLOSE,調整Y的子節點a的h值，h(a)=h(Y)+Y到子節點a的權重C(Y,a),比較a點是否存在於OPEN和CLOSE中，方法如下：用A或其它算法計算，這裏假設用A算法,遍歷Y的子節點，點放入CLOSE,調整Y的子節點a的h值，h(a)=h(Y)+Y到子節點a的權重C(Y,a),比較a點是否存在於OPEN和CLOSE中，方法如下：

while()
{
從OPEN表中取k值最小的節點Y;
遍歷Y的子節點a,計算a的h值 h(a)=h(Y)+Y到子節點a的權重C(Y,a)
{
    if(a in OPEN)     比較兩個a的h值 
    if( a的h值小於OPEN表a的h值 )
    {
更新OPEN表中a的h值;k值取最小的h值
          有未受影響的最短路經存在
          break; 
    }
    if(a in CLOSE) 比較兩個a的h值 //注意是同一個節點的兩個不同路徑的估價值
    if( a的h值小於CLOSE表的h值 )
    {
更新CLOSE表中a的h值; k值取最小的h值;將a節點放入OPEN表
       有未受影響的最短路經存在
       break;
    }
    if(a not in both)
        將a插入OPEN表中;　//還沒有排序
}
放Y到CLOSE表；
OPEN表比較k值大小進行排序；
}

8.D*算法實現（Python）

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time    : 2018/12/13 0013 22:30
# @Author  : 心一
# @Site    : 
# @File    : D_star.py
# @Software: PyCharm

import math
from sys import maxsize # 導入最大數，2^63-1


class State(object):

    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None
        self.state = "."
        self.t = "new"
        self.h = 0
        self.k = 0  # k即爲f

    def cost(self, state):
        if self.state == "#" or state.state == "#":
            return maxsize  # 存在障礙物時，距離無窮大
        return math.sqrt(math.pow((self.x - state.x), 2) +
                         math.pow((self.y - state.y), 2))

    def set_state(self, state):
        if state not in ["S", ".", "#", "E", "*","+"]:
            return
        self.state = state


class Map(object):
    '''
    創建地圖
    '''
    def __init__(self, row, col):
        self.row = row
        self.col = col
        self.map = self.init_map()

    def init_map(self):
        # 初始化map
        map_list = []
        for i in range(self.row):
            tmp = []
            for j in range(self.col):
                tmp.append(State(i, j))
            map_list.append(tmp)
        return map_list

    def print_map(self):
        for i in range(self.row):
            tmp = ""
            for j in range(self.col):
                tmp += self.map[i][j].state + " "
            print(tmp)

    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
                if state.x + i < 0 or state.x + i >= self.row:
                    continue
                if state.y + j < 0 or state.y + j >= self.col:
                    continue
                state_list.append(self.map[state.x + i][state.y + j])
        return state_list

    def set_obstacle(self, point_list):
        # 設置障礙物的位置
        for x, y in point_list:
            if x < 0 or x >= self.row or y < 0 or y >= self.col:
                continue
            self.map[x][y].set_state("#")


class Dstar(object):

    def __init__(self, maps):
        self.map = maps
        self.open_list = set()  # 創建空集合

    def process_state(self):
        '''
        D*算法的主要過程
        :return:
        '''
        x = self.min_state()    # 獲取open list列表中最小k的節點
        if x is None:
            return -1
        k_old = self.get_kmin() #獲取open list列表中最小k節點的k值
        self.remove(x)  # 從openlist中移除
        # 判斷openlist中
        if k_old < x.h:
            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:
            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:
            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))
                else:
                    if y.parent != x and y.h > x.h + x.cost(y):
                        self.insert(y, x.h)
                    else:
                        if 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()

    def min_state(self):
        if not self.open_list:
            return None
        min_state = min(self.open_list, key=lambda x: x.k)  # 獲取openlist中k值最小對應的節點
        return min_state

    def get_kmin(self):
        # 獲取openlist表中k(f)值最小的k
        if not self.open_list:
            return -1
        k_min = min([x.k for x in self.open_list])
        return k_min

    def insert(self, state, h_new):
        if state.t == "new":
            state.k = h_new
        elif state.t == "open":
            state.k = min(state.k, h_new)
        elif state.t == "close":
            state.k = min(state.h, h_new)
        state.h = h_new
        state.t = "open"
        self.open_list.add(state)

    def remove(self, state):
        if state.t == "open":
            state.t = "close"
        self.open_list.remove(state)

    def modify_cost(self, x):
        if x.t == "close":  # 是以一個openlist，通過parent遞推整條路徑上的cost
            self.insert(x, x.parent.h + x.cost(x.parent))

    def run(self, start, 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 = s.parent
            s.set_state("+")
        s.set_state("E")
        print('障礙物未發生變化時，搜索的路徑如下：')
        self.map.print_map()
        tmp = start # 起始點不變
        self.map.set_obstacle([(9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8)]) # 障礙物發生變化
        '''
        從起始點開始，往目標點行進，當遇到障礙物時，重新修改代價，再尋找路徑
        '''
        while tmp != end:
            tmp.set_state("*")
            # self.map.print_map()
            # print("")
            if tmp.parent.state == "#":
                self.modify(tmp)
                continue
            tmp = tmp.parent
        tmp.set_state("E")
        print('障礙物發生變化時，搜索的路徑如下(*爲更新的路徑)：')
        self.map.print_map()

    def modify(self, state):
        '''
        當障礙物發生變化時，從目標點往起始點回推，更新由於障礙物發生變化而引起的路徑代價的變化
        :param state:
        :return:
        '''
        self.modify_cost(state)
        while True:
            k_min = self.process_state()
            if k_min >= state.h:
                break


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)
    # m.print_map()

運行效果如下：

9.算法總結

相比A-star算法，D-star的主要特點就是由目標位置開始向起始位置進行路徑搜索，當物體由起始位置向目標位置運行過程中，發現路徑中存在新的障礙時，對於目標位置到新障礙之間的範圍內的路徑節點，新的障礙是不會影響到其到目標的路徑的。新障礙只會影響的是物體所在位置到障礙之間範圍的節點的路徑。在這時通過 將新的障礙周圍的節點加入到Openlist中進行處理然後向物體所在位置進行傳播，能最小程度的減少計算開銷。
D*路徑搜索的過程和Dijkstra算法比較像，A-star算法中f(n)=g(n)+h(n)，h(n)在D-star中並沒有體現，路徑的搜索並沒有A-star所具有的方向感，即朝着目標搜索的感覺，這種搜索更多的是一種由目標位置向四周發散搜索，直到把起始位置納入搜索範圍爲止，更像是Dijkstra算法。
同時，由示例的算法效果來看，D_star算法能夠在障礙物發生變化時，仍能找到一條路徑，但不一定是一條最短的路徑。

參考資料

[1] Wiki百科：D*
[2] 最短路經算法簡介(Dijkstra算法，A算法，D算法)（轉載）
[3] D star路徑搜索算法
[4] Optimal and Efficient Path Planning for Partially-Known Environments.pdf