在本文中，我們將主要介紹Dijkstra算法和A*算法，從成本計算的角度出發(fā)，并逐步展開討論。

我們將從廣度優(yōu)先搜索開始，然后引入Dijkstra算法，與貪心算法進行比較，最終得出A*算法。

成本計算

在路徑規(guī)劃中，成本計算的一個主要因素是距離。距離可以作為一種衡量路徑長短的度量指標，通常使用歐幾里得距離、曼哈頓距離或其他合適的距離度量方法來計算。

本文主要介紹歐幾里得距離與曼哈頓距離。

廣度優(yōu)先搜索

廣度優(yōu)先搜索（Breadth First Search，BFS ）是一種圖遍歷算法，按照廣度方向逐層遍歷所有可達節(jié)點。

BFS的基本思想是通過維護一個隊列，逐層訪問節(jié)點。

具體步驟如下：

1、將起始節(jié)點放入隊列中，并標記為已訪問。

2、當隊列非空時，執(zhí)行以下步驟：

從隊列中取出一個節(jié)點，記為當前節(jié)點，并標記為已訪問。

如果該節(jié)點是目標節(jié)點，則返回結(jié)果。

將當前節(jié)點的所有未訪問過的鄰居節(jié)點放入隊列中。

3、如果隊列為空，則表示已經(jīng)遍歷完所有可達節(jié)點，算法結(jié)束。

算法框圖

實現(xiàn)效果如下：

廣度優(yōu)先搜索是一種基本的圖搜索算法，它按照圖的廣度方向逐層遍歷所有可達節(jié)點。然而，BFS并不考慮邊的權(quán)重，它只關注節(jié)點的層級關系。

因此，對于成本計算來說，BFS并不適用。這里為了實現(xiàn)到目標點的搜索，采用了曼哈頓距離計算初始點的行進成本。

代碼

def searching(self):
  """
    Breadth-first Searching.
     path, visited order
    """


  self.PARENT[self.s_start] = self.s_start # 開始節(jié)點的父節(jié)點
  self.g[self.s_start] = 0 # 開始節(jié)點的成本
  self.g[self.s_goal] = math.inf # 目標節(jié)點的成本
  # 統(tǒng)一成本搜索，起點的成本是0
  heapq.heappush(self.OPEN,
          (0, self.s_start))


  while self.OPEN:
    _, s = heapq.heappop(self.OPEN) # 彈出最小的元素，優(yōu)先級較高
    self.CLOSED.append(s) # 將節(jié)點加入被訪問元素隊列，已訪問


    if s == self.s_goal: # 到達目標點，即停止
      break


      for s_n in self.get_neighbor(s): # 得到s的鄰居節(jié)點
        new_cost = self.g[s] + self.cost(s, s_n) 
        # 計算當前鄰居節(jié)點s_n的成本=g(s)節(jié)點s的成本+s到s_n之間的成本


        if s_n not in self.g: # 當前節(jié)點沒有訪問過
          self.g[s_n] = math.inf # 起點到節(jié)點s_n的成本為無窮


          if new_cost < self.g[s_n]: ?# conditions for updating Cost
 ? ? ? ? ? ? ? ? ? ? ? ?self.g[s_n] = new_cost
 ? ? ? ? ? ? ? ? ? ? ? ?self.PARENT[s_n] = s


 ? ? ? ? ? ? ? ? ? ? ? ?# bfs, add new node to the end of the openset
 ? ? ? ? ? ? ? ? ? ? ? ?# 將新的節(jié)點添加到隊列的末尾
 ? ? ? ? ? ? ? ? ? ? ? ?prior = self.OPEN[-1][0] + 1 if len(self.OPEN) > 0 else 0
            heapq.heappush(self.OPEN, (prior, s_n))
            self.f[s_n] = prior


            return self.extract_path(self.PARENT), self.CLOSED, self.f

Dijkstra算法

迪杰斯特拉算法(Dijkstra)算法是一種單源最短路徑算法，用于在加權(quán)圖中找到從起點到所有其他節(jié)點的最短路徑。

它基于貪心策略，每次選擇當前距離起點最近的節(jié)點，并通過該節(jié)點更新與它相鄰的節(jié)點的距離。具體步驟如下：

1、初始化：初始化變量和數(shù)據(jù)結(jié)構(gòu)，創(chuàng)建一個包含所有節(jié)點的集合，并為每個節(jié)點設置一個距離值。將起始節(jié)點的父節(jié)點設置為自身，將起始節(jié)點的距離值設置為0，其他節(jié)點的距離值設置為無窮大（表示尚未找到最短路徑）。將起始節(jié)點以成本0的優(yōu)先級推入優(yōu)先隊列OPEN中。

2、主循環(huán)：當OPEN非空時：

彈出優(yōu)先級最小（成本最低）的節(jié)點(_, s)，其中_為忽略的值，s為當前節(jié)點。

將當前節(jié)點s添加到CLOSED列表中，表示已訪問。

檢查當前節(jié)點是否為目標節(jié)點。如果是，則跳出循環(huán)。

對于當前節(jié)點的所有鄰居節(jié)點，計算通過當前節(jié)點到達鄰居節(jié)點的距離，并與鄰居節(jié)點的當前距離值進行比較。

如果計算得到的距離值小于鄰居節(jié)點的當前距離值，則更新鄰居節(jié)點的距離值為新的更小值并將鄰居節(jié)點s_n以新的成本作為優(yōu)先級推入優(yōu)先隊列OPEN中循環(huán)結(jié)束后，可以通過從目標節(jié)點回溯到起始節(jié)點，在PARENT字典中提取最短路徑。

3、循環(huán)結(jié)束后，可以通過從目標節(jié)點回溯到起始節(jié)點，在PARENT字典中提取最短路徑。

算法框圖

實現(xiàn)效果如下：

Dijkstra算法能夠正確地找到起始節(jié)點到其他所有節(jié)點的最短路徑。它基于貪婪策略，每次選擇當前最短路徑的節(jié)點，通過逐步更新節(jié)點的距離值，最終找到最短路徑。

代碼

  def searching(self):
    """
    Breadth-first Searching.
     path, visited order
    """


    self.PARENT[self.s_start] = self.s_start # 開始節(jié)點的父節(jié)點
    self.g[self.s_start] = 0 # 開始節(jié)點的成本
    self.g[self.s_goal] = math.inf # 目標節(jié)點的成本


    # 統(tǒng)一成本搜索，起點的成本是0
    heapq.heappush(self.OPEN,
            (0, self.s_start))


    while self.OPEN: # open_list
      _, s = heapq.heappop(self.OPEN) # 彈出最小的元素，優(yōu)先級較高
      self.CLOSED.append(s) # 將節(jié)點加入被訪問元素隊列


      if s == self.s_goal: # 到達目標點，即停止
        break


      for s_n in self.get_neighbor(s): # 得到s的鄰居節(jié)點
        new_cost = self.g[s] + self.cost(s, s_n) # 計算當時鄰居節(jié)點s_n的成本=g(s)節(jié)點s的成本+s到s_n之間的成本


        if s_n not in self.g:  # 當前節(jié)點沒有訪問過
          self.g[s_n] = math.inf # 起點到節(jié)點s_n的成本為無窮


        if new_cost < self.g[s_n]: ?# 預估節(jié)點s_n成本

	

	貪婪算法

	貪婪算法（Greedy Algorithm）是一種常見的算法設計策略，其基本思想是在每一步選擇當前最優(yōu)解，而不考慮整體的最優(yōu)解。貪婪算法通常以局部最優(yōu)解為目標，通過不斷做出局部最優(yōu)選擇來達到整體最優(yōu)解。

	貪婪算法在路徑規(guī)劃問題中，根據(jù)當前位置到目標位置的成本作為啟發(fā)式評估準則，選擇最近的節(jié)點作為下一步移動的目標。具體步驟如下：

	1、初始化：設置起始節(jié)點，將起始節(jié)點的父節(jié)點設置為起始節(jié)點本身，并將起始節(jié)點和目標節(jié)點的成本初始化為無窮大，將起始節(jié)點加入開放列表，其優(yōu)先級根據(jù)啟發(fā)式函數(shù)值確定。

	2、主循環(huán)：當OPEN非空時：

	從OPEN列表中彈出具有最高優(yōu)先級的節(jié)點，將其加入已訪問列表（CLOSED）中。

	檢查當前節(jié)點是否為目標節(jié)點。如果是，則跳出循環(huán)。

	獲取當前節(jié)點的鄰居節(jié)點，從鄰居節(jié)點中選擇距離目標節(jié)點最近的節(jié)點，將選擇的節(jié)點加入OPEN列表，并將該節(jié)點作為當前節(jié)點。

	3、循環(huán)結(jié)束后，通過從目標節(jié)點回溯到起始節(jié)點，在PARENT字典中提取最短路徑。

	算法框圖

	

	實現(xiàn)效果如下：

	

	貪婪最佳優(yōu)先搜索算法的局限性在于它過度依賴啟發(fā)式函數(shù)（heuristic function），該函數(shù)用于估計節(jié)點到目標節(jié)點的距離。

	由于啟發(fā)式函數(shù)的估計可能不準確或不全面，算法可能會在搜索過程中陷入局部最優(yōu)解，導致得到的路徑并不是最短的。

	代碼

	
 def searching(self):
    self.PARENT[self.s_start] = self.s_start # 開始節(jié)點的父節(jié)點
    self.h[self.s_start] = math.inf # 開始節(jié)點的成本
    self.h[self.s_goal] = math.inf # 目標節(jié)點的成本
    # heappush 函數(shù)能夠按照 h 值的大小來維護堆的順序，這意味著self.OPEN堆中的節(jié)點將按照 h 值的升序排列，h 值較小的節(jié)點將具有較高的優(yōu)先級。
    heapq.heappush(self.OPEN,
            (self.heuristic(self.s_start), self.s_start))


    while self.OPEN: # 當不為空時，即存在未探索區(qū)域
      _, s = heapq.heappop(self.OPEN) # 彈出最小的元素，優(yōu)先級較高
      self.CLOSED.append(s) # 將節(jié)點加入被訪問元素隊列


      if s == self.s_goal: # stop condition，到達目標點，即停止
        break
      for s_n in self.get_neighbor(s): # 得到s的鄰居節(jié)點
        new_cost = self.heuristic(s_n) + self.cost(s, s_n) # 計算當時鄰居節(jié)點s_n的成本=g(s)節(jié)點s的成本+s到s_n之間的成本


        if s_n not in self.h: # 下一個節(jié)點沒有遍歷過
          self.h[s_n] = math.inf # 起點到節(jié)點s_n的成本為無窮


        if new_cost < self.h[s_n]: ?# 預估節(jié)點s_n成本

	

	A*算法

	Dijkstra算法沒有考慮到目標節(jié)點的位置，因此可能會浪費時間在探索那些與目標節(jié)點相距較遠的方向上。貪婪最佳優(yōu)先搜索算法會優(yōu)先選擇離目標節(jié)點更近的節(jié)點進行擴展。

	這樣做的好處是它能夠更快地找到到達目標節(jié)點的路徑，但無法保證找到的路徑是最短路徑，因為它只考慮了節(jié)點到目標節(jié)點的距離，沒有綜合考慮到起點到目標節(jié)點的實際距離。

	A*算法是一種綜合了Dijkstra算法和貪婪最佳優(yōu)先搜索的啟發(fā)式搜索算法。A*算法同時使用了節(jié)點到起點的實際距離（表示為g值）和節(jié)點到目標節(jié)點的估計距離（表示為h值）。

	它通過綜合考慮這兩個值來評估節(jié)點的優(yōu)先級，并選擇優(yōu)先級最高的節(jié)點進行擴展。

	A算法通過選擇合適的啟發(fā)式函數(shù)來平衡搜索的速度和路徑的優(yōu)劣。當啟發(fā)式函數(shù)滿足一定條件時，A算法能夠保證找到最短路徑。

	Dijkstra與貪婪搜索算法對比

	在路徑規(guī)劃中，貪婪算法關注的是當前節(jié)點到目標節(jié)點的距離（啟發(fā)式函數(shù)值），它傾向于選擇離目標節(jié)點最近的節(jié)點作為下一步。

	Dijkstra算法關注的是從起點到各個節(jié)點的距離，通過不斷更新節(jié)點的最短距離來逐步擴展路徑。

	

	A*算法的成本函數(shù)是由兩部分組成：g(n)和h(n)。

	g(n)表示從起點到達節(jié)點n的實際距離（也稱為已知最短路徑的代價），表示為g(n)?！狣ijkstra

	h(n)表示從節(jié)點n到目標節(jié)點的預估距離（也稱為啟發(fā)式函數(shù)），表示為h(n)?！澙匪阉?/p>

	A算法使用這兩個值來評估節(jié)點的優(yōu)先級。具體地，A算法為每個節(jié)點計算一個估計總代價f(n)，計算公式為：

	

	其中，f(n)表示從起點經(jīng)過節(jié)點n到達目標節(jié)點的預估總代價。

	

	具體步驟如下：

	1、初始化：設置起始節(jié)點，將起始節(jié)點的父節(jié)點設置為起始節(jié)點本身，將起始節(jié)點的成本設置為0，將目標節(jié)點的成本設置為無窮大，將起始節(jié)點加入到OPEN列表中，使用節(jié)點的f值作為優(yōu)先級。

	2、主循環(huán)：當OPEN非空時：

	從OPEN列表中彈出具有最高優(yōu)先級的節(jié)點，將其加入已訪問列表（CLOSED）中。 

	檢查當前節(jié)點是否為目標節(jié)點。如果是，則跳出循環(huán)。 

	獲取當前節(jié)點的鄰居節(jié)點。

	 對于每個鄰居節(jié)點，執(zhí)行以下步驟：

	計算從起始節(jié)點經(jīng)過當前節(jié)點到達鄰居節(jié)點的實際距離，即g值。 

	如果鄰居節(jié)點不在g字典中，將其g值初始化為無窮大。 

	如果計算得到的g值小于鄰居節(jié)點的當前g值，更新鄰居節(jié)點的g值為新的更小值，并將當前節(jié)點設為鄰居節(jié)點的父節(jié)點。 

	計算鄰居節(jié)點的啟發(fā)式函數(shù)值，即h值。 

	將鄰居節(jié)點加入OPEN列表，并根據(jù)f值（f = g + h）確定其優(yōu)先級。

	3、循環(huán)結(jié)束后，通過從目標節(jié)點回溯到起始節(jié)點，在PARENT字典中提取最短路徑。

	算法框圖

	

	實現(xiàn)效果如下：

	

	A*算法的效率和質(zhì)量受啟發(fā)式函數(shù)的選擇影響較大。合理選擇啟發(fā)式函數(shù)能夠提供更好的搜索引導，但不同問題可能需要設計不同的啟發(fā)式函數(shù)。

	代碼

	
def searching(self):
    """
    A_star Searching.
     path, visited order
    """


    self.PARENT[self.s_start] = self.s_start # 開始節(jié)點的父節(jié)點
    self.g[self.s_start] = 0 # 開始節(jié)點的成本
    self.g[self.s_goal] = math.inf # 目標節(jié)點的成本


    # heappush 函數(shù)能夠按照 f 值的大小來維護堆的順序，這意味著self.OPEN堆中的節(jié)點將按照 f 值的升序排列，f 值較小的節(jié)點將具有較高的優(yōu)先級。
    heapq.heappush(self.OPEN,
            (self.f_value(self.s_start), self.s_start))


    while self.OPEN: # 當不為空時，即存在未探索區(qū)域
      _, s = heapq.heappop(self.OPEN) # 彈出最小的元素，優(yōu)先級較高
      self.CLOSED.append(s) # 將節(jié)點加入被訪問元素隊列


      if s == self.s_goal: # stop condition，到達目標點，即停止
        break
      for s_n in self.get_neighbor(s): # 得到s的鄰居節(jié)點
        new_cost = self.g[s] + self.cost(s, s_n) # 計算當時鄰居節(jié)點s_n的成本=g(s)節(jié)點s的成本+s到s_n之間的成本


        if s_n not in self.g:
          self.g[s_n] = math.inf # 起點到節(jié)點s_n的成本為無窮


        if new_cost < self.g[s_n]: ?# 預估節(jié)點s_n成本