路徑規劃| 圖搜尋演算法：DFS - BFS、GBFS、Dijkstra

... First Search, DFS)、廣度優先搜尋(Breadth First Search, BFS)。

... 引入一種啟發式搜尋演算法：貪婪最佳優先演算法(Greedy Best First Search, ... MdEditor 路徑規劃|圖搜尋演算法：DFS、BFS、GBFS、Dijkstra、A*-知乎 語言:CN/TW/HK 時間 2021-01-2717:01:12 osc_2axit9df 主題: c++ 5點講透雲原生安全！LinuxFoundation開源軟體大學開課啦！>>> 地圖資料常常可以用圖(Graph)這類資料結構表示，那麼在圖結構中常用的搜尋演算法也可以應用到路徑規劃中。

本文將從圖搜尋演算法的基本流程入手，層層遞進地介紹幾種圖搜尋演算法。

首先是兩種針對無權圖的基本圖搜尋演算法：深度優先搜尋(DepthFirstSearch,DFS)、廣度優先搜尋(BreadthFirstSearch,BFS)。

它們的區別在於openlist(後面介紹)所選用的資料結構型別不同，前者使用棧，後者使用佇列；之後引入一種啟發式搜尋演算法：貪婪最佳優先演算法(GreedyBestFirstSearch,GBFS)，用來提高搜尋效率，但是不能確保找到最優路徑；最後介紹兩種在路徑規劃中非常經典的演算法：Dijkstra演算法、A*演算法，前者是廣度優先演算法(BFS)在帶權圖中的擴充套件，後者則是在前者中加入啟發函式得到的演算法，兼顧效率和完備性。

配置空間 在學習路徑規劃演算法之前，首先了解一下配置空間(ConfigurationSpace)這個概念。

在實際環境，也就是機器人的工作空間(Workspace)中，機器人是有形狀和大小的，這不利於進行運動規劃。

要將工作空間轉換到配置空間中，即將機器人轉化為一個質點，同時將障礙物按照機器人的體積進行膨脹，如下圖： 這樣，在進行路徑規劃時，就可以將機器人當做一個點來處理了。

基本流程 下面切入正題，圖搜尋演算法的基本流程如下： 建立一個容器，一般稱為openlist，用來儲存將要訪問的節點 將起點加入容器 開始迴圈： ----彈出：從容器中取出一個節點 ----擴充套件：獲取該節點周圍的節點，將這些節點放入容器 深度優先搜尋(DFS) 深度優先，顧名思義即深度越大的節點會被優先擴充套件。

在DFS中，使用棧(Stack)資料結構來實現上述特性。

棧是一種後進先出(LIFO)的容器，如下圖 以在下面的無權圖中找到從節點a到節點i的路徑為例，說明一下DFS演算法的工作流程 按照上節的圖搜尋演算法的基本流程進行搜尋，過程如下： 從i回溯得到路徑：a->b->c->g->i，如下： DFS能夠快速地找到一條路徑，是一種以時間換空間的方法。

將其應用到二維地圖的路徑規劃中，如下圖，很顯然找到的路徑並不是移動機器人運動規劃所需要的最優路徑 廣度優先搜尋(BFS) 與DFS的“不撞南牆不回頭”的個性不同，BFS在搜尋時呈波狀推進形式，一路穩紮穩打，它是一種以時間換空間的方法，能夠保證搜尋到的路徑是最優的。

為了實現波狀推進搜尋特性，BFS採用佇列(Queue)作為openlist的資料結構。

佇列是一種先進先出(FIFO)的容器，如下圖 其流程與上節中DFS類似，繼續以上節的圖舉例，過程如下，首先建立一個佇列作為容器，將節點a加入佇列 接著將節點a彈出佇列，將節點a周圍沒有訪問過的節點加入佇列 按照上面的流程不斷地彈出、擴充套件節點，直到找到節點i為止，完整流程如下圖： 從終點回溯，i的父節點為f，f的父節點為e，e的父節點為a，這樣就可以得到a到i的最短路徑為：a->e->f->i，如下 顯而易見，相較於DFS，BFS中使用了大量的入隊、出隊操作，耗時增加，但是能保證找到最優路徑。

啟發式搜尋演算法 BFS和DFS的區別主要在於節點的彈出策略，根據彈出策略的區別，分別使用了佇列和棧兩種資料結構，而棧和佇列作為兩種相當基本的容器，只將節點進入容器的順序作為彈出節點的依據，並未考慮目標位置等因素，這就使搜尋過程變得漫無目的，導致效率低下。

啟發式搜尋演算法(HeuristicAlgorithm)就是用來解決搜尋效率問題的，下面將以貪婪最佳優先演算法(GreedyBestFirstSearch,GBFS)為例來介紹啟發式搜尋演算法。

GBFS也是圖搜尋演算法的一種，它的演算法流程和BFS、DFS並沒有本質的不同，區別仍然在於openlist採用的資料結構，GBFS使用的是優先佇列(PriorityQueue)，普通佇列是一種先進先出的資料結構，而在優先佇列中元素被賦予了優先順序，最高優先順序元素優先刪除，也就是firstin,largestout。

(記住這種資料結構，後面的Dijkstra和A*演算法都會用到這個結構)。

在圖搜尋演算法中，使用代價函式來作為優先順序判斷的標準，越小，優先順序越高，反之優先順序越低。

GBFS作為一種啟發式搜尋演算法，使用啟發評估函式來作為代價函式，也就是 其中是當前節點到終點的代價，它可以指引搜尋演算法往終點靠近，主要用歐氏距離(EuclideanDistance)或者曼哈頓距離(ManhattanDistance)來表示，它們的區別如下圖： 假設有兩個點和，則它們的歐氏距離和曼哈頓距離分別為： 將GBFS應用在二維地圖路徑規劃中，如下圖，可以看到它的指向性或者說目的非常明顯，從起點直撲終點。

但是在實際的地圖中，常常會有很多障礙物，它就很容易陷入區域性最優的陷阱。

下圖的地圖中有一個專門設定的區域性最優陷阱，很顯然GBFS雖然搜尋速度夠快，但是找不到最優路徑。

將其應用到複雜二維地圖路徑規劃中，效果如下： Dijkstra演算法 上面的演算法中，只有廣度優先搜尋(BFS)具有完備性，能夠保證搜尋到最優路徑。

但是可以看到BFS演算法搜尋到的路徑只有向上/下/左/右移動這四個動作，它們是沒有權值或者說權值都相同的，只能用於無權圖的路徑規劃，無法實現能夠對角移動的路徑規劃。

因此下面介紹一種能用於帶權圖的圖搜尋演算法——Dijkstra演算法(狄克斯特拉演算法)。

Dijkstra演算法是從一個頂點到其餘各頂點的最短路徑演算法，其流程仍然與上述演算法基本一致，它也是用優先佇列作為openlist的資料結構，它和GBFS的區別在於代價函式的定義，Dijkstra算的定義為： 其中表示從起點到當前點的移動代價。

以下圖為例，計算起點a到終點i的最短路徑，箭頭上的數值表示兩個節點間的距離 首先擴充套件第一個節點，計算其餘節點與第一個節點的距離，用橙色標出已經擴充套件的節點，未擴充套件的節點仍用綠色標出，其中圓中的數值表示該節點的代價函式，字母則表示該節點沒有直接到達此時已擴充套件節點的路徑。

從未擴充套件的節點(綠色節點)中選擇代價函式最小的節點進行拓展，並更新其餘節點的代價函式，如下圖 重複進行上面的步驟，直到所有節點都已擴充套件。

最後標出起點到終點的最短路徑 將Dijkstra演算法應用到二維地圖路徑規劃中，如下圖，可以看到Dijkstra演算法能夠得到最優路徑，但是它的速度和BFS是一樣的，採取的都是穩紮穩打、波狀前進的方式，導致速度較慢。

A*演算法 對比GBFS和Dijkstra演算法，兩者都採用優先佇列作為openlist，而代價函式的不同導致兩者具有不同的優點：GBFS用節點到目標點的距離作為代價函式，將搜尋方向引向目標點，搜尋效率高；而Dijkstra演算法採用起點到當前擴充套件節點的移動代價作為代價函式，能夠確保路徑最優。

那麼可不可以將兩者的代價函式進行融合，從而在保證路徑最優的同時提高搜尋效率？答案是肯定的，融合後的演算法就是A*演算法。

A*演算法也是一種啟發式演算法，它的代價函式表示為： 其中為起點到當前擴充套件節點的移動代價函式，是啟發函式，用節點到目標點的距離函式來表示。

根據這個式子，可以得到A*演算法的幾個特點： 如果令，A*演算法就退化為Dijkstra演算法；如果令，A*演算法就退化為GBFS演算法。

能否找到最優路徑的關鍵是啟發函式的選取，如果在大部分情況下比從當前節點到目標點的移動代價小，則能找到最優路徑。

由於A*演算法的啟發函式是位置上的距離，因此在不帶位置資訊的圖資料中不適用。

將A*演算法應用到二維地圖路徑規劃中，如下圖： 附錄：圖基礎 本節附錄將介紹什麼是圖資料結構，以及圖資料結構在計算機中的表示方法。

圖結構 圖結構是一種由資料元素集合及元素間的關係集合組成的非線性資料結構。

資料元素用節點(node)表示，元素間的關係用邊(edge)表示，如果兩個元素相關，就用一條邊將相應的節點連線起來，這兩個節點稱為相鄰節點。

根據邊的方向性，可以分為無向圖和有向圖，一個無向圖G記做，其中是節點的有限集合，是邊的有限集合，則有： 上式中，表示節點x和節點y之間的相鄰關係，是一種無序節點對，無方向性，稱為連線節點x和節點y的一條邊。

如果節點間關係是有序節點對，則用表示，表示從節點x到節點y的一條單向邊，是有方向的，則有向邊的集合可表示為： 下圖分別為無向圖和有向圖的圖形化示例： 上圖中的無向圖可以表示為： 有向圖可表示為： 在圖中除了用邊表示兩個節點之間的相鄰關係外，有時還需要表示它們相關的強度資訊，例如從一個節點到另一個節點的距離、花費的代價、所需的時間等，諸如此類的資訊可以通過在圖的每條邊上加上一個稱作權(weight)的數值表示，這類圖稱為帶權圖 上述就是圖結構的基本概念，我們還需要知道圖結構在計算機中的表示方法 圖結構的鄰接矩陣表示法 鄰接矩陣用來表示圖的邊集，即節點間的相鄰關係集合。

設是一個具有n個節點的圖，它的鄰接矩陣是一個n階矩陣，則其中的元素滿足： 對於無向圖，其鄰接矩陣是對稱矩陣，即，而有向圖的鄰接矩陣不一定對稱，其空間複雜度均為。

以下為兩個不帶權圖的鄰接矩陣示例： 對於帶權圖，設或者上的權值為，則帶權圖的鄰接矩陣定義為： 以下為兩個帶權圖的鄰接矩陣示例： 圖結構的鄰接表表示法 鄰接矩陣表示法的空間複雜度為，其佔用的空間大小與圖中節點數量相關，而與邊的數目無關。

對於稀疏圖，其邊的數量可能遠小於，這樣鄰接矩陣中就會有很多零或元素。

對於這種情況，可以用節點表和鄰接表來表示和儲存圖結構，其佔用的儲存空間既與圖的節點數有關，也與邊數有關。

圖的節點表用來儲存圖中的所有節點，通常是一個順序儲存的線性表，該線性表中的每個元素對應圖中的一個節點，該節點型別包括兩個基本成員：節點資料元素資訊data和指向該節點的鄰接表neighbors。

對於無向圖有： 參考 圖的實現（c++）_y1054765649的部落格-CSDN部落格_c++圖​blog.csdn.net 機器人路徑規劃之Dijkstra演算法_白鳥無言的部落格-CSDN部落格​blog.csdn.net 「其他文章」 蜂巢無鈷正極材料批量下線：首批無鈷電池即將量產 【612頁】Android大廠面試題及解析大全（中高階） Filter-Policy 路徑規劃|圖搜尋演算法：DFS、BFS、GBFS、Dijkstra、A*-知乎 神奇的交際圈！這位17世紀的法國神父結交的好朋友，竟然都是一流數學牛人：笛卡爾、費馬、加森迪······ PostgreSQL備份工具pgBackRest使用 自學系列|就談自我管理！ Ansible之自動化部署redis主從(單機) 年輕人，請別再亂花錢了！ HTML基礎知識 Flutter異常捕獲和Crash崩潰日誌收集 Pi-Cardia的ShortCut™器械成功治療首批患者 ACM金牌選手帶你練習C實戰專案 用哪種語言寫的應用漏洞最嚴重？六大主流語言程式碼漏洞分析報告出爐 HTML和 完全不用命令列 clouderaaes加密解決一定要用jdk而不要用預設的oraclejdk否則hue Ceph日誌分析 面試演算法實踐與國外大廠習題指南 計劃超越蘋果！諾基亞在印度推出膝上型電腦 「c++」 Masonry幾個妙用 尤大大說我的程式碼全部不加分號|重學JS 為了學（mo）習（yu），我竟開發了這樣一個外掛 前端效能優化——圖片篇 99.9%的人都不知道的箭頭函式不能當做建構函式的祕密 揭祕BPFmap前生今世 為什麼越來越多的科技公司都選擇了WebRTC技術？ C結構體指標初始化 使用Vite開發前端庫實踐 從B端到C端，預製菜正在全面包圍年輕人的餐桌，對頗具儀式感的年夜飯更是虎視眈眈。