博客轉載自：https://blog.csdn.net/kongbu0622/article/details/1871520

據 Drew 所知最短路經算法現在重要的應用有計算機網絡路由算法，機器人探路，交通路線導航，人工智能，遊戲設計等等。美國火星探測器核心的尋路算法就是采用的D*（D Star）算法。最短路經計算分靜態最短路計算和動態最短路計算。靜態路徑最短路徑算法是外界環境不變，計算最短路徑。主要有Dijkstra算法，A*（A Star）算法。 動態路徑最短路是外界環境不斷發生變化，即不能計算預測的情況下計算最短路。如在遊戲中敵人或障礙物不斷移動的情況下。典型的有D*算法。

這是Drew程序實現的10000個節點的隨機路網三條互不相交最短路

真實路網計算K條路徑示例：節點5696到節點3006，三條最快速路，可以看出路徑基本上走環線或主幹路。黑線為第一條，蘭線為第二條，紅線為第三條。約束條件系數為1.2。共享部分路段。 顯示計算部分完全由Drew自己開發的程序完成。

參見 K條路算法測試程序

Dijkstra算法求最短路徑

Dijkstra算法是典型最短路算法，用於計算一個節點到其他所有節點的最短路徑。主要特點是以起始點為中心向外層層擴展，直到擴展到終點為止。Dijkstra算法能得出最短路徑的最優解，但由於它遍歷計算的節點很多，所以效率低。Dijkstra算法是很有代表性的最短路算法，在很多專業課程中都作為基本內容有詳細的介紹，如數據結構，圖論，運籌學等等。Dijkstra一般的表述通常有兩種方式，一種用永久和臨時

大概過程：
創建兩個表，OPEN, CLOSE。OPEN表保存所有已生成而未考察的節點，CLOSED表中記錄已訪問過的節點。

1． 訪問路網中裏起始點最近且沒有被檢查過的點，把這個點放入OPEN組中等待檢查。
2． 從OPEN表中找出距起始點最近的點，找出這個點的所有子節點，把這個點放到CLOSE表中。
3． 遍歷考察這個點的子節點。求出這些子節點距起始點的距離值，放子節點到OPEN表中。
4． 重復2，3，步。直到OPEN表為空，或找到目標點。

這是在drew 程序中4000個節點的隨機路網上Dijkstra算法搜索最短路的演示，黑色圓圈表示經過遍歷計算過的點由圖中可以看到Dijkstra算法從起始點開始向周圍層層計算擴展，在計算大量節點後，到達目標點。所以速度慢效率低。提高Dijkstra搜索速度的方法很多，據Drew所知，常用的有數據結構采用Binary heap的方法，和用Dijkstra從起始點和終點同時搜索的方法。

推薦網頁：http://www.cs.ecnu.edu.cn/assist/js04/ZJS045/ZJS04505/zjs045050a.htm，簡明扼要介紹Dijkstra算法，有圖解顯示和源碼下載。

A*算法 -- 啟發式（heuristic）算法

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算法的毫無無方向的向四周搜索。

主要搜索過程：
創建兩個表，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的大小，從最小路徑的節點向下進行。
}

上圖是和上面Dijkstra算法使用同一個路網，相同的起點終點，用A*算法的情況，計算的點數從起始點逐漸向目標點方向擴展，計算的節點數量明顯比Dijkstra少得多，效率很高，且能得到最優解。A*算法和Dijistra算法的區別在於有無估價值，Dijistra算法相當於A*算法中估價值為0的情況。

推薦文章鏈接：Amit 斯坦福大學一個博士的遊戲網站，上面有關於A*算法介紹和不少有價值的鏈接 http://theory.stanford.edu/~amitp/GameProgramming/

Sunway寫的兩篇很好的介紹啟發式和A*算法的中文文章並有A*源碼下載：初識A*算法 http://creativesoft.home.shangdu.net/AStart1.htm和深入A*算法 http://creativesoft.home.shangdu.net/AStart2.htm

需要註意的是Sunway上面文章“深入A*算法”中引用了一個A*的遊戲程序進行講解，並有這個源碼的下載，不過它有一個不小的Bug, 就是新的子節點放入OPEN表中進行了排序，而當子節點在Open表和Closed表中時，重新計算估價值後，沒有重新的對Open表中的節點排序，這個問題會導致計算有時得不到最優解，另外在路網權重懸殊很大時，搜索範圍不但超過Dijkstra，甚至搜索全部路網, 使效率大大降低。Drew 對這個問題進行了如下修正，當子節點在Open表和Closed表中時，重新計算估價值後，刪除OPEN表中的老的節點，將有新估價值的節點插入OPEN表中，重新排序，經測試效果良好，修改的代碼如下，紅色部分為Drew添加的代碼.添加進程序的相應部分即可。

在函數GenerateSucc（）中 ...................................

g=BestNode->g+1; /* g(Successor)=g(BestNode)+cost of getting from BestNode to Successor */
TileNumS=TileNum((int)x,(int)y); /* identification purposes */
if ((Old=CheckOPEN(TileNumS)) != NULL) 
{ 
for(c=0;c<8;c++)
if(BestNode->Child[c] == NULL) /* Add Old to the list of BestNode‘s Children (or Successors). */
break;
BestNode->Child[c]=Old;

if (g < Old->g) 
{
Old->Parent=BestNode;
Old->g=g;
Old->f=g+Old->h;

 
//Drew 在該處添加如下紅色代碼 
//Implement by Drew 
NODE *q,*p=OPEN->NextNode, *temp=OPEN->NextNode;
while(p!=NULL && p->NodeNum != Old->NodeNum)
{
    q=p;
    p=p->NextNode;
}
if(p->NodeNum == Old->NodeNum)
{
   if(p==OPEN->NextNode)
  {
     temp = temp->NextNode;
     OPEN ->NextNode = temp;
  }
  else
  q->NextNode = p->NextNode;
 }
Insert(Old); // Insert Successor on OPEN list wrt f 
} 

......................................................

A*（A Star）算法與D算法的轉換

這種算法可以不直接用估價值，直接用Dijkstra算法程序實現A*算法，Drew對它進行了測試，達到和A*完全一樣的計算效果，且非常簡單。以鄰接矩陣為例，更改原來鄰接矩陣i行j列元素Dij為 Dij+Djq-Diq; 起始點到目標點的方向i->j, 終點q. Dij為（i到j路段的權重或距離）其中：Djq,Diq的作用相當於估價值 Djq=（j到q的直線距離）；Diq=（i到q的直線距離）原理：i 到q方向符合Dij+Djq > Diq ，取Dij+Djq-Diq 小，如果是相反方向Dij+Djq-Diq會很大。因此達到向目標方向尋路的作用。

動態路網 -- 最短路徑算法 D*

A* 在靜態路網中非常有效（very efficient for static worlds），但不適於在動態路網，環境如權重等不斷變化的動態環境下。 D*是動態A*（D-Star,Dynamic A Star） 卡內及梅隆機器人中心的Stentz在1994和1995年兩篇文章提出，主要用於機器人探路。是火星探測器采用的尋路算法，見附錄1，2.

主要方法（這些完全是Drew在讀了上述資料和編制程序中的個人理解，不能保證完全正確，僅供參考）

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*算法在動態環境中尋路非常有效，向目標點移動中，只檢查最短路徑上下一節點或臨近節點的變化情況，如機器人尋路等情況。對於距離遠的最短路徑上發生的變化，則感覺不太適用。

上圖是Drew在4000個節點的隨機路網上做的分析演示，細黑線為第一次計算出的最短路，紅點部分為路徑上發生變化的堵塞點，當機器人位於982點時，檢測到前面發生路段堵塞，在該點重新根據新的信息計算路徑，可以看到圓圈點為重新計算遍歷過的點，僅僅計算了很少得點就找到了最短路，說明計算非常有效，迅速。綠線為計算出的繞開堵塞部分的新的最短路徑。

附錄

1. Optimal and Efficient Path Planning for Partially-Known Environments

2. The Focussed D* Algorithm for Real-Time Replanning

人工智能: 自動尋路算法實現(四、D、D*算法)