Ⅰ 最短路徑演算法
Dijkstra演算法，A*演算法和D*演算法
Dijkstra演算法是典型最短路演算法，用於計算一個節點到其他所有節點的最短路徑。主要特點是以起始點為中心向外層層擴展，直到擴展到終點為止。Dijkstra演算法能得出最短路徑的最優解，但由於它遍歷計算的節點很多，所以效率低。
Dijkstra演算法是很有代表性的最短路演算法，在很多專業課程中都作為基本內容有詳細的介紹，如數據結構，圖論，運籌學等等。
Dijkstra一般的表述通常有兩種方式，一種用永久和臨時標號方式，一種是用OPEN, CLOSE表方式，Drew為了和下面要介紹的 A* 演算法和 D* 演算法表述一致，這里均採用OPEN,CLOSE表的方式。
大概過程：
創建兩個表，OPEN, CLOSE。
OPEN表保存所有已生成而未考察的節點，CLOSED表中記錄已訪問過的節點。
1． 訪問路網中里起始點最近且沒有被檢查過的點，把這個點放入OPEN組中等待檢查。
2． 從OPEN表中找出距起始點最近的點，找出這個點的所有子節點，把這個點放到CLOSE表中。
3． 遍歷考察這個點的子節點。求出這些子節點距起始點的距離值，放子節點到OPEN表中。
4． 重復2，3，步。直到OPEN表為空，或找到目標點。
提高Dijkstra搜索速度的方法很多，常用的有數據結構採用Binary heap的方法，和用Dijkstra從起始點和終點同時搜索的方法。
A*（A-Star)演算法是一種啟發式演算法，是靜態路網中求解最短路最有效的方法。
公式表示為： f(n)=g(n)+h(n),
其中f(n) 是節點n從初始點到目標點的估價函數，
g(n) 是在狀態空間中從初始節點到n節點的實際代價，
h(n)是從n到目標節點最佳路徑的估計代價。
保證找到最短路徑（最優解的）條件，關鍵在於估價函數h(n)的選取：
估價值h(n)<= n到目標節點的距離實際值，這種情況下，搜索的點數多，搜索范圍大，效率低。但能得到最優解。
如果 估價值>實際值, 搜索的點數少，搜索范圍小，效率高，但不能保證得到最優解。
估價值與實際值越接近，估價函數取得就越好。
例如對於幾何路網來說，可以取兩節點間歐幾理德距離（直線距離）做為估價值，即f=g(n)+sqrt((dx-nx)*(dx-nx)+(dy-ny)*(dy-ny))；這樣估價函數f在g值一定的情況下，會或多或少的受估價值h的制約，節點距目標點近，h值小，f值相對就小，能保證最短路的搜索向終點的方向進行。明顯優於Dijstra演算法的毫無無方向的向四周搜索。
conditions of heuristic
Optimistic (must be less than or equal to the real cost)
As close to the real cost as possible
主要搜索過程：
創建兩個表，OPEN表保存所有已生成而未考察的節點，CLOSED表中記錄已訪問過的節點。
遍歷當前節點的各個節點，將n節點放入CLOSE中，取n節點的子節點X,->算X的估價值->
While(OPEN!=NULL)
{
從OPEN表中取估價值f最小的節點n;
if(n節點==目標節點) break;
else
{
if(X in OPEN) 比較兩個X的估價值f //注意是同一個節點的兩個不同路徑的估價值
if( X的估價值小於OPEN表的估價值 )
更新OPEN表中的估價值; //取最小路徑的估價值
if(X in CLOSE) 比較兩個X的估價值 //注意是同一個節點的兩個不同路徑的估價值
if( X的估價值小於CLOSE表的估價值 )
更新CLOSE表中的估價值; 把X節點放入OPEN //取最小路徑的估價值
if(X not in both)
求X的估價值;
並將X插入OPEN表中; //還沒有排序
}
將n節點插入CLOSE表中;
按照估價值將OPEN表中的節點排序; //實際上是比較OPEN表內節點f的大小，從最小路徑的節點向下進行。
}
A*演算法和Dijistra演算法的區別在於有無估價值，Dijistra演算法相當於A*演算法中估價值為0的情況。
動態路網，最短路演算法 D*A* 在靜態路網中非常有效（very efficient for static worlds），但不適於在動態路網，環境如權重等不斷變化的動態環境下。
D*是動態A*（D-Star,Dynamic A*） 卡內及梅隆機器人中心的Stentz在1994和1995年兩篇文章提出，主要用於機器人探路。是火星探測器採用的尋路演算法。
主要方法：
1.先用Dijstra演算法從目標節點G向起始節點搜索。儲存路網中目標點到各個節點的最短路和該位置到目標點的實際值h,k（k為所有變化h之中最小的值,當前為k=h。每個節點包含上一節點到目標點的最短路信息1(2),2(5),5(4)，4（7）。則1到4的最短路為1-2-5-4。
原OPEN和CLOSE中節點信息保存。
2.機器人沿最短路開始移動，在移動的下一節點沒有變化時，無需計算，利用上一步Dijstra計算出的最短路信息從出發點向後追述即可，當在Y點探測到下一節點X狀態發生改變，如堵塞。機器人首先調整自己在當前位置Y到目標點G的實際值h(Y)，h(Y)=X到Y的新權值c(X,Y)+X的原實際值h(X).X為下一節點(到目標點方向Y->X->G），Y是當前點。k值取h值變化前後的最小。
3.用A*或其它演算法計算，這里假設用A*演算法,遍歷Y的子節點，點放入CLOSE,調整Y的子節點a的h值，h(a)=h(Y)+Y到子節點a的權重C(Y,a),比較a點是否存在於OPEN和CLOSE中，方法如下：
while()
{
從OPEN表中取k值最小的節點Y;
遍歷Y的子節點a,計算a的h值 h(a)=h(Y)+Y到子節點a的權重C(Y,a)
{
if(a in OPEN) 比較兩個a的h值
if( a的h值小於OPEN表a的h值 )
{ 更新OPEN表中a的h值;k值取最小的h值
有未受影響的最短路經存在
break;
}
if(a in CLOSE) 比較兩個a的h值 //注意是同一個節點的兩個不同路徑的估價值
if( a的h值小於CLOSE表的h值 )
{
更新CLOSE表中a的h值; k值取最小的h值;將a節點放入OPEN表
有未受影響的最短路經存在
break;
}
if(a not in both)
將a插入OPEN表中; //還沒有排序
}
放Y到CLOSE表；
OPEN表比較k值大小進行排序；
}
機器人利用第一步Dijstra計算出的最短路信息從a點到目標點的最短路經進行。
D*演算法在動態環境中尋路非常有效，向目標點移動中，只檢查最短路徑上下一節點或臨近節點的變化情況，如機器人尋路等情況。對於距離遠的最短路徑上發生的變化，則感覺不太適用。