- 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為起點的單源最短路徑步驟如下：
具體實現的程式碼如下：
#include <stdio.h> #include <string.h> #define MAX_LEN 100 #define INFINITE 1000 typedef struct graph { int nodenum; // 頂點數 int edgenum; // 邊數 int matrix[MAX_LEN][MAX_LEN]; // 儲存圖的鄰接矩陣 }Graph; typedef struct stack // 定義堆疊結構體型別 { int top; int printout[MAX_LEN]; }Stack; void InitStack(Stack *s) // 初始化堆疊 { s->top = -1; // 棧為空時棧頂指標top=-1 memset(s->printout,0,sizeof(int)*MAX_LEN); // 對記憶體塊進行置零 } void push(Stack *s,int m) // 入棧 { s->printout[++(s->top)] = m; } int pop(Stack *s) // 出棧 { return s->printout[s->top--]; } int in[MAX_LEN]; // 若in[i] = 1,則說明頂點vi已在集合S中 int dist[MAX_LEN]; // dist[i]是"頂點vs"到"頂點i"的最短路徑的長度 int prev[MAX_LEN]; // prev[i]的值是"頂點vs"到"頂點i"的最短路徑所經歷的全部頂點中,位於"頂點i"之前的那個頂點 void InitGraph(Graph *g,int n) { int i, j; for(i = 1; i <= n; i++) // 初始化鄰接矩陣 { for(j = 1; j <= n;j++) { if(i == j) g->matrix[i][j] = 0; else g->matrix[i][j] = INFINITE; } } for(i=1;i<=n;i++) { in[i] = 0; dist[i] = INFINITE; prev[i] = 0; } } int main() { int n,m,i,I,J,weight,count,min,k,temp; while(scanf("%d %d",&n,&m)) // 輸入頂點數和邊數 { Graph mGraph; mGraph.edgenum = m; mGraph.nodenum = n; InitGraph(&mGraph, n); for(i = 0; i < m; i++) { scanf("%d %d %d", &I, &J, &weight); mGraph.matrix[I][J] = weight; // 根據輸入填充鄰接矩陣 mGraph.matrix[J][I] = weight; } in[1] = 1; // 初始時只包含源頂點1，即S＝{1} prev[1] = 1; dist[1] = 0; // 1到1的距離為0 for(i = 2; i <= n; i++) { dist[i] = mGraph.matrix[1][i]; // 其它各點到頂點vs的距離 if(dist[i] != INFINITE) prev[i] = 1; } count = 0; while(count < n-1) // 遍歷n-1次 { min = INFINITE; for(i = 1; i <= n; i++) { if(in[i] == 0 && dist[i] < min) // 查詢最近點(從U中選取一個距離vs最小的頂點k) { min = dist[i]; k = i; } } in[k] = 1; // 標記查詢到的最近點,加入S中 for(i = 1; i <= n; i++) // update the distance { if( in[i]==0 && (min + mGraph.matrix[k][i])<dist[i] ) // 判斷是vs直接連v[i]短,還是經過v[k]連線v[i]更短 { dist[i] = min + mGraph.matrix[k][i]; // 更新dist prev[i] = k; // 記錄前驅頂點 } } count++; } Stack s; for(i = 1; i <= n; i++) { temp = i; InitStack(&s); if(prev[temp] == 0) { printf("no path\n"); continue; } while(prev[temp] != 1) { push(&s, prev[temp]); temp = prev[temp]; } printf("1-->"); while(s.top != -1) // 判斷棧是否為空 { printf("%d-->", pop(&s)); // 輸出最短路徑 } printf("%d min length is %d\n", i, dist[i]); } } getchar(); return 0; }
輸入和輸出如下圖所示：
- A*演算法
路徑規劃是指的是機器人的最優路徑規劃問題，即依據某個或某些優化準則（如工作代價最小、行走路徑最短、行走時間最短等），在工作空間中找到一個從起始狀態到目標狀態能避開障礙物的最優路徑。機器人的路徑規劃應用場景極豐富，最常見如遊戲中NPC及控制角色的位置移動，百度地圖等導航問題，小到家庭掃地機器人、無人機大到各公司正爭相開拓的無人駕駛汽車等。
目前路徑規劃演算法分為：
A*演算法原理：
在電腦科學中，A*演算法作為Dijkstra演算法的擴充套件，因其高效性而被廣泛應用於尋路及圖的遍歷，如星際爭霸等遊戲中就大量使用。在理解演算法前，我們需要知道幾個概念：
- 搜尋區域（The Search Area）：圖中的搜尋區域被劃分為了簡單的二維陣列，陣列每個元素對應一個小方格，當然我們也可以將區域等分成是五角星，矩形等，通常將一個單位的中心點稱之為搜尋區域節點（Node）。
- 開放列表(Open List)：我們將路徑規劃過程中待檢測的節點存放於Open List中，而已檢測過的格子則存放於Close List中。
- 父節點（parent）：在路徑規劃中用於回溯的節點，開發時可考慮為雙向連結串列結構中的父結點指標。
- 路徑排序（Path Sorting）：具體往哪個節點移動由以下公式確定：F(n) = G + H 。G代表的是從初始位置A沿著已生成的路徑到指定待檢測格子的移動開銷。H指定待測格子到目標節點B的估計移動開銷。
- 啟發函式（Heuristics Function）：H為啟發函式，也被認為是一種試探，由於在找到唯一路徑前，我們不確定在前面會出現什麼障礙物，因此用了一種計算H的演算法，具體根據實際場景決定。在我們簡化的模型中，H採用的是傳統的曼哈頓距離（Manhattan Distance），也就是橫縱向走的距離之和。
如下圖所示，綠色方塊為機器人起始位置A，紅色方塊為目標位置B，藍色為障礙物。
我們把要搜尋的區域劃分成了正方形的格子。這是尋路的第一步，簡化搜尋區域。這個特殊的方法把我們的搜尋區域簡化為了 2 維陣列。陣列的每一項代表一個格子，它的狀態就是可走 (walkalbe)或不可走 (unwalkable) 。現用A*演算法尋找出一條自A到B的最短路徑，每個方格的邊長為10，即垂直水平方向移動開銷為10。因此沿對角移動開銷約等於14。具體步驟如下：
從起點 A 開始，把它加入到一個由方格組成的open list(開放列表) 中，這個open list像是一個購物清單。Open list裡的格子是可能會是沿途經過的，也有可能不經過。因此可以將其看成一個待檢查的列表。檢視與A相鄰的8個方格 ，把其中可走的 (walkable) 或可到達的(reachable) 方格加入到open list中。並把起點 A 設定為這些方格的父節點 (parent node) 。然後把 A 從open list中移除，加入到close list(封閉列表) 中，close list中的每個方格都是不需要再關注的。
如下圖所示，深綠色的方格為起點A，它的外框是亮藍色，表示該方格被加入到了close list 。與它相鄰的黑色方格是需要被檢查的，他們的外框是亮綠色。每個黑方格都有一個灰色的指標指向他們的父節點A。
下一步，我們需要從open list中選一個與起點A相鄰的方格。但是到底選擇哪個方格好呢？選F值最小的那個。我們看看下圖中的一些方格。在標有字母的方格中G = 10 。這是因為水平方向從起點到那裡只有一個方格的距離。與起點直接相鄰的上方，下方，左方的方格的 G 值都是 10 ，對角線的方格 G 值都是14 。H值通過估算起點到終點 ( 紅色方格 ) 的 Manhattan 距離得到，僅作橫向和縱向移動，並且忽略沿途的障礙。使用這種方式，起點右邊的方格到終點有3 個方格的距離，因此 H = 30 。這個方格上方的方格到終點有 4 個方格的距離 ( 注意只計算橫向和縱向距離 ) ，因此 H = 40 。
比較open list中節點的F值後，發現起點A右側節點的F=40，值最小。選作當前處理節點，並將這個點從Open List刪除，移到Close List中。
對這個節點周圍的8個格子進行判斷，若是不可通過（比如牆，水，或是其他非法地形）或已經在Close List中，則忽略。否則執行以下步驟：
- 若當前處理節點的相鄰格子已經在Open List中，則檢查這條路徑是否更優，即計算經由當前處理節點到達那個方格是否具有更小的 G值。如果沒有，不做任何操作。相反，如果G值更小，則把那個方格的父節點設為當前處理節點 ( 我們選中的方格 ) ，然後重新計算那個方格的 F 值和 G 值。
- 若當前處理節點的相鄰格子不在Open List中，那麼把它加入，並將它的父節點設定為該節點。
按照上述規則我們繼續搜尋，選擇起點右邊的方格作為當前處理節點。它的外框用藍線打亮，被放入了close list 中。然後我們檢查與它相鄰的方格。它右側的3個方格是牆壁，我們忽略。它左邊的方格是起點，在 close list 中，我們也忽略。其他4個相鄰的方格均在 open list 中，我們需要檢查經由當前節點到達那裡的路徑是否更好。我們看看上面的方格，它現在的G值為14 ，如果經由當前方格到達那裡， G值將會為20( 其中10為從起點到達當前方格的G值，此外還要加上從當前方格縱向移動到上面方格的G值10) ，因此這不是最優的路徑。看圖就會明白直接從起點沿對角線移動到那個方格比先橫向移動再縱向移動要好。
當把4個已經在 open list 中的相鄰方格都檢查後，沒有發現經由當前節點的更好路徑，因此不做任何改變。接下來要選擇下一個待處理的節點。因此再次遍歷open list ，現在open list中只有 7 個方格了，我們需要選擇F值最小的那個。這次有兩個方格的F值都是54，選哪個呢？沒什麼關係。從速度上考慮，選擇最後加入 open list 的方格更快。因此選擇起點右下方的方格，如下圖所示。
接下來把起點右下角F值為54的方格作為當前處理節點，檢查其相鄰的方格。我們發現它右邊是牆（牆下面的一格也忽略掉，假定牆角不能直接穿越)，忽略之。這樣還剩下 5 個相鄰的方格。當前方格下面的 2 個方格還沒有加入 open list ，所以把它們加入，同時把當前方格設為他們的父親。在剩下的 3 個方格中，有 2 個已經在 close list 中 ( 一個是起點，一個是當前方格上面的方格，外框被加亮的 ) ，我們忽略它們。最後一個方格，也就是當前方格左邊的方格，檢查經由當前方格到達那裡是否具有更小的 G 值。沒有，因此我們準備從 open list 中選擇下一個待處理的方格。
不斷重複這個過程，直到把終點也加入到了 open list 中，此時如下圖所示。注意在起點下方 2 格處的方格的父親已經與前面不同了。之前它的G值是28並且指向它右上方的方格。現在它的 G 值為 20 ，並且指向它正上方的方格。這是由於在尋路過程中的某處使用新路徑時G值更小，因此父節點被重新設定，G和F值被重新計算。
那麼我們怎樣得到實際路徑呢？很簡單，如下圖所示，從終點開始，沿著箭頭向父節點移動，直至回到起點，這就是你的路徑。
A*演算法總結：
1. 把起點加入 open list 。
2. 重複如下過程：
a. 遍歷open list ，查詢F值最小的節點，把它作為當前要處理的節點，然後移到close list中
b. 對當前方格的 8 個相鄰方格一一進行檢查，如果它是不可抵達的或者它在close list中，忽略它。否則，做如下操作：
□ 如果它不在open list中，把它加入open list，並且把當前方格設定為它的父親
□ 如果它已經在open list中，檢查這條路徑 ( 即經由當前方格到達它那裡 ) 是否更近。如果更近，把它的父親設定為當前方格，並重新計算它的G和F值。如果你的open list是按F值排序的話，改變後你可能需要重新排序。
c. 遇到下面情況停止搜尋：
□ 把終點加入到了 open list 中，此時路徑已經找到了，或者
□ 查詢終點失敗，並且open list 是空的，此時沒有路徑。
3. 從終點開始，每個方格沿著父節點移動直至起點，形成路徑。
根據演算法描述，虛擬碼如下：
function A*(start, goal) // The set of nodes already evaluated. closedSet := {} // The set of currently discovered nodes still to be evaluated. Initially, only the start node is known. openSet := {start} // For each node, which node it can most efficiently be reached from. // If a node can be reached from many nodes, cameFrom will eventually contain the most efficient previous step. cameFrom := the empty map // For each node, the cost of getting from the start node to that node. gScore := map with default value of Infinity // The cost of going from start to start is zero. gScore[start] := 0 // For each node, the total cost of getting from the start node to the goal // by passing by that node. That value is partly known, partly heuristic. fScore := map with default value of Infinity // For the first node, that value is completely heuristic. fScore[start] := heuristic_cost_estimate(start, goal) while openSet is not empty current := the node in openSet having the lowest fScore[] value if current = goal return reconstruct_path(cameFrom, current) openSet.Remove(current) closedSet.Add(current) for each neighbor of current if neighbor in closedSet continue // Ignore the neighbor which is already evaluated. // The distance from start to a neighbor tentative_gScore := gScore[current] + dist_between(current, neighbor) if neighbor not in openSet // Discover a new node openSet.Add(neighbor) else if tentative_gScore >= gScore[neighbor] continue // This is not a better path. // This path is the best until now. Record it! cameFrom[neighbor] := current gScore[neighbor] := tentative_gScore fScore[neighbor] := gScore[neighbor] + heuristic_cost_estimate(neighbor, goal) return failure
function reconstruct_path(cameFrom, current) total_path := [current] while current in cameFrom.Keys: current := cameFrom[current] total_path.append(current) return total_path
根據上面的虛擬碼，用Python實現一個簡單的最短路徑搜尋。使用二維陣列表示地圖，其中1表示有障礙節點，0表示無障礙節點。
import numpy from heapq import heappush,heappop def heuristic_cost_estimate(neighbor, goal): x = neighbor[0] - goal[0] y = neighbor[1] - goal[1] return abs(x) + abs(y) def dist_between(a, b): return (b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2 def reconstruct_path(came_from, current): path = [current] while current in came_from: current = came_from[current] path.append(current) return path # astar function returns a list of points (shortest path) def astar(array, start, goal): directions = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)] # 8個方向 close_set = set() came_from = {} gscore = {start:0} fscore = {start:heuristic_cost_estimate(start, goal)} openSet = [] heappush(openSet, (fscore[start], start)) # 往堆中插入一條新的值 # while openSet is not empty while openSet: # current := the node in openSet having the lowest fScore value current = heappop(openSet)[1] # 從堆中彈出fscore最小的節點 if current == goal: return reconstruct_path(came_from, current) close_set.add(current) for i, j in directions: # 對當前節點的 8 個相鄰節點一一進行檢查 neighbor = current[0] + i, current[1] + j ## 判斷節點是否在地圖範圍內，並判斷是否為障礙物 if 0 <= neighbor[0] < array.shape[0]: if 0 <= neighbor[1] < array.shape[1]: if array[neighbor[0]][neighbor[1]] == 1: # 1為障礙物 continue else: # array bound y walls continue else: # array bound x walls continue # Ignore the neighbor which is already evaluated. if neighbor in close_set: continue # The distance from start to a neighbor via current tentative_gScore = gscore[current] + dist_between(current, neighbor) if neighbor not in [i[1] for i in openSet]: # Discover a new node heappush(openSet, (fscore.get(neighbor, numpy.inf), neighbor)) elif tentative_gScore >= gscore.get(neighbor, numpy.inf): # This is not a better path. continue # This path is the best until now. Record it! came_from[neighbor] = current gscore[neighbor] = tentative_gScore fscore[neighbor] = tentative_gScore + heuristic_cost_estimate(neighbor, goal) return False if __name__ == "__main__": nmap = numpy.array([ [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,1,1,1,1,1,1,1,1,1,1,1,0,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,0,1,1,1,1,1,1,1,1,1,1,1,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,1,1,1,1,1,1,1,1,1,1,1,0,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,0,1,1,1,1,1,1,1,1,1,1,1,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,1,1,1,1,1,1,1,1,1,1,1,0,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0]]) path = astar(nmap, (0,0), (10,13)) for i in range(len(path)): nmap[path[i]] = 100
使用Spyder IDE可以在Variable explorer中檢視和修改二維陣列，陣列中的值根據大小以不同顏色顯示。將搜尋到的路徑節點賦予一個較大的值，可以直觀地看出從起點到終點的路徑。
使用Pillow、OpenCV或Matplotlib等影象處理庫，可以在自己繪製的圖片上進行尋路：
# -*- coding: utf-8 -*- import numpy as np from heapq import heappush,heappop def heuristic_cost_estimate(neighbor, goal): x = neighbor[0] - goal[0] y = neighbor[1] - goal[1] return abs(x) + abs(y) def dist_between(a, b): return (b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2 def reconstruct_path(came_from, current): path = [current] while current in came_from: current = came_from[current] path.append(current) return path # astar function returns a list of points (shortest path) def astar(array, start, goal): directions = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)] # 8個方向 close_set = set() came_from = {} gscore = {start:0} fscore = {start:heuristic_cost_estimate(start, goal)} openSet = [] heappush(openSet, (fscore[start], start)) # 往堆中插入一條新的值 # while openSet is not empty while openSet: # current := the node in openSet having the lowest fScore value current = heappop(openSet)[1] # 從堆中彈出fscore最小的節點 if current == goal: return reconstruct_path(came_from, current) close_set.add(current) for i, j in directions: # 對當前節點的 8 個相鄰節點一一進行檢查 neighbor = current[0] + i, current[1] + j ## 判斷節點是否在地圖範圍內，並判斷是否為障礙物 if 0 <= neighbor[0] < array.shape[0]: if 0 <= neighbor[1] < array.shape[1]: if array[neighbor[0]][neighbor[1]] == 0: # 0為障礙物 continue else: # array bound y walls continue else: # array bound x walls continue # Ignore the neighbor which is already evaluated. if neighbor in close_set: continue # The distance from start to a neighbor via current tentative_gScore = gscore[current] + dist_between(current, neighbor) if neighbor not in [i[1] for i in openSet]: # Discover a new node heappush(openSet, (fscore.get(neighbor, np.inf), neighbor)) elif tentative_gScore >= gscore.get(neighbor, np.inf): # This is not a better path. continue # This path is the best until now. Record it! came_from[neighbor] = current gscore[neighbor] = tentative_gScore fscore[neighbor] = tentative_gScore + heuristic_cost_estimate(neighbor, goal) return False if __name__ == "__main__": from PIL import Image import matplotlib.pyplot as plt img = Image.open('C:\Users\Administrator\Desktop\map.bmp') map = np.array(img) # 影象轉化為二維陣列 path = astar(map, (0,0), (260,260)) # 繪製路徑 img = np.array(img.convert('RGB')) for i in range(len(path)): img[path[i]] = [0,0,255] plt.imshow(img) plt.axis('off') plt.show()
在畫圖程式中繪製一個300×300畫素的地圖，填充黑色表示障礙，並將其存為灰度圖。計算出路徑後轉換為彩色圖，並繪製出路線：
上面產生的路徑貼著障礙物邊緣，如果對實際機器人或者遊戲中的角色進行路徑規劃，要考慮其實際大小，將地圖上的障礙物尺寸擴大（inflate），避免與障礙物發生碰撞。
參考：
A*，Dijkstra，BFS演算法效能比較及A*演算法的應用
http://theory.stanford.edu/~amitp/GameProgramming/
PYTHON A* PATHFINDING (WITH BINARY HEAP)
http://www.policyalmanac.org/games/aStarTutorial.htm
A-Star (A*) Implementation in C# (Path Finding, PathFinder)
Implementing A Star (A*) Path Planning Algorithm On A 3-DOF PR2 Robot In Python