路徑規劃—詳解Dijkstra演算法
目錄
一、什麼是Dijstra演算法
定義：採用優先順序定義的廣度優先搜尋思想，以拓撲連通圖的起始點為中心按照路徑長度以遞增方式層層向外擴充套件，直到擴充套件到終止點。
二、如何實現Dijstra演算法
1、思路：
拓撲連通圖由節點和連線邊組成。其中節點有兩個屬性，分別為當前節點到起始點的最小路徑長度dist和父節點parent_node；連線邊具有路徑長度edge.cost。首先訪問拓撲連通圖中的起始點，然後反覆檢查最接近但未檢查的節點，並將其鄰域結點新增到要檢查的節點。
2、步驟：
（1）建立節點與起始點的距離矩陣distanceFromStart，將起始點的值設定為0，其餘元素設定為無窮大inf
（2）建立節點的父節點矩陣parent，初始化為0，表示無父節點
（3）每次迭代更新時，找出distanceFromStart中未完成遍歷節點中總評估成本最低的節點，設定為當前節點current_node；設定current_node.dist=inf，（防止再次遍歷），並標記為已完成遍歷；對curren_node的四個相鄰節點中的未遍歷節點進行判定，判斷相鄰節點N.g否大於current_node.g+edge.cost，如果大於，則N.g=current_node.g+edge.cost，N.parent_node=current_node
（4）重複步驟（3）直至current_node為目標點為止
（5）根據父節點矩陣parent回溯最優路徑
3、例子：
在如下圖所示的離散拓撲連通圖中，要求解從起始點A到目標點G的最佳路徑。在下面的求解過程中，為了方便顯示，使用綠色表示起始點，橙色表示目標點，紅色代表節點已經完成遍歷，藍色表示節點尚未完成遍歷，白色表示該節點尚未納入考慮範圍。
在第一次迭代中A是路徑長度最小的節點，所以A設定為當前節點current_node，並標記為完成遍歷。將A從T中移除，加入到S。然後對A開始第一輪搜尋，與A相鄰的節點為B和E。對於B來說，B.dist=inf>A.dist+edge_cost=0+2，因此更新B.dist=2，B的parent為A。對於E來說，E.dist=inf>A.dist+edge_cost=0+3，因此更新E.dist=3，E的parent為A。此時S={A->A=0}，T={A->B=2, A->C=inf, A->D=inf,A->E=3, A->F=inf,A->G=inf}。
在第二次迭代中，從T中選擇路徑最短的節點B為current_node，並標記為完成遍歷。將B從T中移除，加入S。然後對B進行搜尋，與B相鄰的節點為C。對於C來說，C.dist=inf>B.dist+edge_cost=2+2，因此更新C.dist=4，C的parent為B。此時S={A->A=0,A->B=2}，T={A->C=4(路徑ABC), A->D=inf,A->E=3, A->F=inf,A->G=inf}。
在第三次迭代中，從T中選擇路徑最短的節點E為current_node，並標記為完成遍歷。將E從T中移除，加入S。然後對E進行搜尋，與E相鄰的未遍歷節點為C和F。對於C來說，C.dist=4<E.dist+edge_cost=3+7，因此保持原樣。對於F來說，F.dist=inf<E.dist+edge_cost=3+3，因此更新F.dist=6，F的parent為E。此時S={A->A=0,A->B=2,A->E=3}，T={A->C=4(路徑ABC), A->D=inf, A->F=6(路徑AEF),A->G=inf}。
在第四次迭代中，從T中選擇路徑最短的節點C為current_node，並標記為完成遍歷。將C從T中移除，加入S。然後對C進行搜尋，與C相鄰的未遍歷節點為D和F。對於D來說，D.dist=inf>C.dist+edge_cost=4+3，因此更新D.dist=7，D的parent為C。對於F來說，F.dist=6<C.dist+edge_cost=4+3，因此保持原樣。此時S={A->A=0,A->B=2,A->E=3, A->C=4(路徑ABC)}，T={A->D=7(路徑ABCD), A->F=6(路徑AEF),A->G=inf}。
在第五次迭代中，從T中選擇路徑最短的節點F為current_node，並標記為完成遍歷。將F從T中移除，加入S。然後對F進行搜尋，與F相鄰的未遍歷節點為G。對於G來說，G.dist=inf>F.dist+edge_cost=6+3，因此更新G.dist=9，G的parent為F。此時S={A->A=0,A->B=2,A->E=3, A->C=4(路徑ABC), A->F=6(路徑AEF)}，T={A->D=7(路徑ABCD),A->G=9(路徑AEFG)}。
在第六次迭代中，從T中選擇路徑最短的節點D為current_node，並標記為完成遍歷。將D從T中移除，加入S。然後對D進行搜尋，與D相鄰的未遍歷節點為G。對於G來說，G.dist=9>F.dist+edge_cost=3+4，因此更新G.dist=7，G的parent為D。此時S={A->A=0,A->B=2,A->E=3, A->C=4(路徑ABC), A->F=6(路徑AEF) ,A->D=7(路徑ABCD)}，T={A->G=9(路徑AEFG)}。
在第七次迭代中，從T中選擇路徑最短的節點G為current_node，由於G為目標節點，因此結束迭代。此時根據節點的parent屬性回溯（G-D-C-B-A），可得出最短路徑A->B->C->D->G。
三、Matlab模擬效果
在Matlab軟體中建立柵格地圖作為拓撲連通圖，將地圖區域分割為若干個單位區域柵格，分為佔用柵格（機器人不可以自由通過）和非佔用柵格（機器人可以自由通過）。黑色柵格表示障礙物，白色柵格表示自由空間。同時假設機器人在網路中運動且只佔用一個柵格。實驗效果如下，迭代了1363次，具體程式碼在另一篇部落格中詳述（正在整理寫作）：
