---------------------------------------------------------------

##先引用一篇文章 https://www.cnblogs.com/21207-iHome/p/6048969.html

已經不知道原始出處了，。

*? ?**Dijkstra算法**

　　迪杰斯特拉（Dijkstra）算法是典型的最短路徑的算法，由荷蘭計算機科學家迪杰斯特拉于1959年提出，用來求得從起始點到其他所有點最短路徑。該算法采用了貪心的思想，每次都查找與該點距離最近的點，也因為這樣，它不能用來解決存在負權邊的圖。解決的問題可描述為：在無向圖 G=(V,E) 中，假設每條邊 E[i] 的長度為 w[i]，找到由頂點vs到其余各點的最短路徑。

　　1) 基本思想：

　　通過Dijkstra計算圖G中的最短路徑時，需要指定起點vs(即從頂點vs開始計算)。此外，引進兩個集合S和U。S的作用是記錄已求出最短路徑的頂點(以及相應的最短路徑長度)，而U則是記錄還未求出最短路徑的頂點(以及該頂點到起點vs的距離)。初始時，S中只有起點vs；U中是除vs之外的頂點，并且U中頂點的路徑是"起點vs到該頂點的路徑"。然后，從U中找出路徑最短的頂點，并將其加入到S中；接著，更新U中的頂點和頂點對應的路徑。 然后，再從U中找出路徑最短的頂點，并將其加入到S中；接著，更新U中的頂點和頂點對應的路徑。重復該操作，直到遍歷完所有頂點。

　　2) 算法步驟：

　　a.初始時，S只包含源點，即S＝{vs}，vs的距離為0。U包含除vs外的其他頂點，即U={其余頂點}，若u不是vs的出邊鄰接點，則<u,vs>權值為∞；

　　b.從U中選取一個距離vs最小的頂點k，把k加入S中（該選定的距離就是vs到k的最短路徑長度min）；

　　c.以k為新考慮的中間點，修改U中各頂點的距離；若從源點vs到頂點u的距離（經過頂點k）比原來距離（不經過頂點k）短，則修改頂點u的距離值，即dist[u] = min( dist[u], min + w[k][u] )；

　　d.重復步驟b和c直到所有頂點都包含在S中。

　　3) 算法實例：

　　先給出一個無向圖

　　用Dijkstra算法找出以A為起點的單源最短路徑步驟如下：

　　具體實現的代碼如下：

?View Code

　　輸入和輸出如下圖所示：


*? ?**A*算法**

　　路徑規(guī)劃是指的是機器人的最優(yōu)路徑規(guī)劃問題，即依據某個或某些優(yōu)化準則（如工作代價最小、行走路徑最短、行走時間最短等），在工作空間中找到一個從起始狀態(tài)到目標狀態(tài)能避開障礙物的最優(yōu)路徑。機器人的路徑規(guī)劃應用場景極豐富，最常見如游戲中NPC及控制角色的位置移動，百度地圖等導航問題，小到家庭掃地機器人、無人機大到各公司正爭相開拓的無人駕駛汽車等。

　　目前路徑規(guī)劃算法分為：

**　　A*算法原理：**

　　在計算機科學中，A*算法作為Dijkstra算法的擴展，因其高效性而被廣泛應用于尋路及圖的遍歷，如星際爭霸等游戲中就大量使用。在理解算法前，我們需要知道幾個概念：

*? ?搜索區(qū)域（The Search Area）：圖中的搜索區(qū)域被劃分為了簡單的二維數組，數組每個元素對應一個小方格，當然我們也可以將區(qū)域等分成是五角星，矩形等，通常將一個單位的中心點稱之為搜索區(qū)域節(jié)點（Node）?！　?/p>

*? ?開放列表(Open List)：我們將路徑規(guī)劃過程中待檢測的節(jié)點存放于Open List中，而已檢測過的格子則存放于Close List中。

*? ?父節(jié)點（parent）：在路徑規(guī)劃中用于回溯的節(jié)點，開發(fā)時可考慮為雙向鏈表結構中的父結點指針。

*? ?路徑排序（Path Sorting）：具體往哪個節(jié)點移動由以下公式確定：F(n) = G + H 。G代表的是從初始位置A沿著已生成的路徑到指定待檢測格子的移動開銷。H指定待測格子到目標節(jié)點B的估計移動開銷。

*? ?啟發(fā)函數（Heuristics Function）：H為啟發(fā)函數，也被認為是一種試探，由于在找到唯一路徑前，我們不確定在前面會出現什么障礙物，因此用了一種計算H的算法，具體根據實際場景決定。在我們簡化的模型中，H采用的是傳統(tǒng)的曼哈頓距離（Manhattan Distance），也就是橫縱向走的距離之和。

　　如下圖所示，綠色方塊為機器人起始位置A，紅色方塊為目標位置B，藍色為障礙物。

　　我們把要搜尋的區(qū)域劃分成了正方形的格子。這是尋路的第一步，簡化搜索區(qū)域。這個特殊的方法把我們的搜索區(qū)域簡化為了 2 維數組。數組的每一項代表一個格子，它的狀態(tài)就是可走 (walkalbe)或不可走 (unwalkable) 。現用A*算法尋找出一條自A到B的最短路徑，每個方格的邊長為10，即垂直水平方向移動開銷為10。因此沿對角移動開銷約等于14。具體步驟如下：

　　從起點 A 開始，把它加入到一個由方格組成的open list(開放列表) 中，這個open list像是一個購物清單。Open list里的格子是可能會是沿途經過的，也有可能不經過。因此可以將其看成一個待檢查的列表。查看與A相鄰的8個方格 ，把其中可走的 (walkable) 或可到達的(reachable) 方格加入到open list中。并把起點 A 設置為這些方格的父節(jié)點 (parent node) 。然后把 A 從open list中移除，加入到close list(封閉列表) 中，close list中的每個方格都是不需要再關注的。

　　如下圖所示，深綠色的方格為起點A，它的外框是亮藍色，表示該方格被加入到了close list 。與它相鄰的黑色方格是需要被檢查的，他們的外框是亮綠色。每個黑方格都有一個灰色的指針指向他們的父節(jié)點A。

　　下一步，我們需要從open list中選一個與起點A相鄰的方格。但是到底選擇哪個方格好呢？選F值最小的那個。我們看看下圖中的一些方格。在標有字母的方格中G = 10 。這是因為水平方向從起點到那里只有一個方格的距離。與起點直接相鄰的上方，下方，左方的方格的 G 值都是 10 ，對角線的方格 G 值都是14 。H值通過估算起點到終點 ( 紅色方格 ) 的 Manhattan 距離得到，僅作橫向和縱向移動，并且忽略沿途的障礙。使用這種方式，起點右邊的方格到終點有3 個方格的距離，因此 H = 30 。這個方格上方的方格到終點有 4 個方格的距離 ( 注意只計算橫向和縱向距離 ) ，因此 H = 40 。

　　比較open list中節(jié)點的F值后，發(fā)現起點A右側節(jié)點的F=40，值最小。選作當前處理節(jié)點，并將這個點從Open List刪除，移到Close List中。

　　對這個節(jié)點周圍的8個格子進行判斷，若是不可通過（比如墻，水，或是其他非法地形）或已經在Close List中，則忽略。否則執(zhí)行以下步驟：

*? ?若當前處理節(jié)點的相鄰格子已經在Open List中，則檢查這條路徑是否更優(yōu)，即計算經由當前處理節(jié)點到達那個方格是否具有更小的 G值。如果沒有，不做任何操作。相反，如果G值更小，則把那個方格的父節(jié)點設為當前處理節(jié)點 ( 我們選中的方格 ) ，然后重新計算那個方格的 F 值和 G 值。

*? ?若當前處理節(jié)點的相鄰格子不在Open List中，那么把它加入，并將它的父節(jié)點設置為該節(jié)點。

　　按照上述規(guī)則我們繼續(xù)搜索，選擇起點右邊的方格作為當前處理節(jié)點。它的外框用藍線打亮，被放入了close list 中。然后我們檢查與它相鄰的方格。它右側的3個方格是墻壁，我們忽略。它左邊的方格是起點，在 close list 中，我們也忽略。其他4個相鄰的方格均在 open list 中，我們需要檢查經由當前節(jié)點到達那里的路徑是否更好。我們看看上面的方格，它現在的G值為14 ，如果經由當前方格到達那里， G值將會為20( 其中10為從起點到達當前方格的G值，此外還要加上從當前方格縱向移動到上面方格的G值10) ，因此這不是最優(yōu)的路徑?？磮D就會明白直接從起點沿對角線移動到那個方格比先橫向移動再縱向移動要好。

　　當把4個已經在 open list 中的相鄰方格都檢查后，沒有發(fā)現經由當前節(jié)點的更好路徑，因此不做任何改變。接下來要選擇下一個待處理的節(jié)點。因此再次遍歷open list ，現在open list中只有 7 個方格了，我們需要選擇F值最小的那個。這次有兩個方格的F值都是54，選哪個呢？沒什么關系。從速度上考慮，選擇最后加入 open list 的方格更快。因此選擇起點右下方的方格，如下圖所示。

　　接下來把起點右下角F值為54的方格作為當前處理節(jié)點，檢查其相鄰的方格。我們發(fā)現它右邊是墻（墻下面的一格也忽略掉，假定墻角不能直接穿越)，忽略之。這樣還剩下 5 個相鄰的方格。當前方格下面的 2 個方格還沒有加入 open list ，所以把它們加入，同時把當前方格設為他們的父親。在剩下的 3 個方格中，有 2 個已經在 close list 中 ( 一個是起點，一個是當前方格上面的方格，外框被加亮的 ) ，我們忽略它們。最后一個方格，也就是當前方格左邊的方格，檢查經由當前方格到達那里是否具有更小的 G 值。沒有，因此我們準備從 open list 中選擇下一個待處理的方格。

　　不斷重復這個過程，直到把終點也加入到了 open list 中，此時如下圖所示。注意在起點下方 2 格處的方格的父親已經與前面不同了。之前它的G值是28并且指向它右上方的方格?，F在它的 G 值為 20 ，并且指向它正上方的方格。這是由于在尋路過程中的某處使用新路徑時G值更小，因此父節(jié)點被重新設置，G和F值被重新計算。

　　那么我們怎樣得到實際路徑呢？很簡單，如下圖所示，從終點開始，沿著箭頭向父節(jié)點移動，直至回到起點，這就是你的路徑。

**　　A*算法總結：**

　　1\. 把起點加入 open list 。

　　2\. 重復如下過程：

　　　　a. 遍歷open list ，查找F值最小的節(jié)點，把它作為當前要處理的節(jié)點，然后移到close list中

　　　　b. 對當前方格的 8 個相鄰方格一一進行檢查，如果它是不可抵達的或者它在close list中，忽略它。否則，做如下操作：

　　　　□? 如果它不在open list中，把它加入open list，并且把當前方格設置為它的父親

　　　　□? 如果它已經在open list中，檢查這條路徑 ( 即經由當前方格到達它那里 ) 是否更近。如果更近，把它的父親設置為當前方格，并重新計算它的G和F值。如果你的open list是按F值排序的話，改變后你可能需要重新排序。

　　　　c. 遇到下面情況停止搜索：

　　　　□? 把終點加入到了 open list 中，此時路徑已經找到了，或者

　　　　□? 查找終點失敗，并且open list 是空的，此時沒有路徑。

　　3\. 從終點開始，每個方格沿著父節(jié)點移動直至起點，形成路徑。

　　根據算法描述，偽代碼如下：

　　 根據上面的偽代碼，用Python實現一個簡單的最短路徑搜尋。使用二維數組表示地圖，其中1表示有障礙節(jié)點，0表示無障礙節(jié)點。

　　使用Spyder IDE可以在Variable explorer中查看和修改二維數組，數組中的值根據大小以不同顏色顯示。將搜尋到的路徑節(jié)點賦予一個較大的值，可以直觀地看出從起點到終點的路徑。

?　　使用Pillow、OpenCV或Matplotlib等圖像處理庫，可以在自己繪制的圖片上進行尋路：

?　　在畫圖程序中繪制一個300×300像素的地圖，填充黑色表示障礙，并將其存為灰度圖。計算出路徑后轉換為彩色圖，并繪制出路線：

　　上面產生的路徑貼著障礙物邊緣，如果對實際機器人或者游戲中的角色進行路徑規(guī)劃，要考慮其實際大小，將地圖上的障礙物尺寸擴大（inflate），避免與障礙物發(fā)生碰撞。

以上轉載自 https://www.cnblogs.com/21207-iHome/p/6048969.html

----------------------------------------------------------------------------------------------------------------

##我的實現實例：