Dijkstra算法
Dijkstra（迪傑斯特拉）算法是典型的最短路徑路由算法，用于計算一個節點到其他所有節點的最短路徑。主要特點是以起始點為中心向外層層擴充，直到擴充到終點為止（BFS、prime算法都有類似思想）。Dijkstra算法能得出最短路徑的最優解，但由于它周遊計算的節點很多，是以效率低。
缺陷：有向圖不能有負權邊
算法思想：
令G = （V，E）為一個帶權有向網，把圖中的頂點集合V分成兩組：已求出最短路徑的頂點集合S（初始時S中隻有源節點，以後每求得一條最短路徑，就将它對應的頂點加入到集合S中，直到全部頂點都加入到S中）；未确定最短路徑的頂點集合U。
算法描述
（1）S為已經找到的從v出發的最短路徑的終點集合，它的初始狀态為空集，将源點加入S中。 其餘頂點構成集合U。
（2）建構源點到其餘頂點的距離清單，與源點不相連的頂點距離記為∞。
（3）廣度周遊與源點相連的頂點，找到距離最近的頂點，則到這個頂點的最短路徑就确定了，最短距離就是目前距離，将這個頂點從U中拿出，放入S中。
（4）用目前的頂點作為中間點，對其進行廣度周遊，對周遊到的頂點距離進行更新。
（5）在U中搜尋最短距離的頂點，将其放入S。
（6）以這個節點作為中間點廣度搜尋，更新距離。
（7）重複這個過程，直至U為空。
代碼：
（C++）
#include<cstdio>
#include<cstring>
#include<algorithm>
#include<iostream>
#define Inf 0x3f3f3f3f
using namespace std;
int map[1005][1005];
int vis[1005],dis[1005];
int n,m;//n個點，m條邊
void Init ()
{
memset(map,Inf,sizeof(map));
for(int i=1;i<=n;i++)
{
map[i][i]=0;
}
}
void Getmap()
{
int u,v,w;
for(int t=1;t<=m;t++)
{
scanf("%d%d%d",&u,&v,&w);
if(map[u][v]>w)
{
map[u][v]=w;
map[v][u]=w;
}
}
}
void Dijkstra(int u)
{
memset(vis,0,sizeof(vis));
for(int t=1;t<=n;t++)
{
dis[t]=map[u][t];
}
vis[u]=1;
for(int t=1;t<n;t++)
{
int minn=Inf,temp;
for(int i=1;i<=n;i++)
{
if(!vis[i]&&dis[i]<minn)
{
minn=dis[i];
temp=i;
}
}
vis[temp]=1;
for(int i=1;i<=n;i++)
{
if(map[temp][i]+dis[temp]<dis[i])
{
dis[i]=map[temp][i]+dis[temp];
}
}
}
}
int main()
{
scanf("%d%d",&m,&n);
Init();
Getmap();
Dijkstra(n);
printf("%dn",dis[1]);
return 0;
}
A*算法
A*算法是在Dijkstra基礎上的改進。也可以說是Dijkstra和最佳優先搜尋（BFS）的結合，首先看一下Dijkstra和BFS的差別。
上邊是采用Dijkstra計算最短路徑的示意圖，由于Dijkstra是層層向外擴充的，是以搜尋區域很大，時間較慢，但準确度較高，可以保證得到的路徑一定是最短的。
下邊圖是采用BFS進行搜尋的，與Dijkstra不同的是，標明下一個搜尋點并不是根據目前點距離起點的距離最近，而是依據目前點距離終點的距離最近來決定下一個搜尋的點。BFS不能保證找到一條最短路徑。然而，它比Dijkstra算法快的多，因為它用了一個啟發式函數（heuristic function）快速地導向目标結點。
然而，這兩個例子都僅僅是最簡單的情況——地圖中沒有障礙物，最短路徑是直線的。現在我們來考慮前邊描述的凹型障礙物。Dijkstra算法運作得較慢，但确實能保證找到一條最短路徑：
1968年發明的A*算法就是把啟發式方法（heuristic approaches）如BFS，和正常方法如Dijsktra算法結合在一起的算法。有點不同的是，類似BFS的啟發式方法經常給出一個近似解而不是保證最佳解。然而，盡管A*基于無法保證最佳解的啟發式方法，A*卻能保證找到一條最短路徑。
A*算法對于距離的判斷不僅僅是起點到目前頂點的距離，同時也加入了目前頂點到終點距離的預估，即：
F = G + H
其中G表示起點到目前頂點的距離，H表示目前頂點到終點距離的預估，我們一般用曼哈頓距離來表示這個估計，假設目前頂點的坐标為（Ax，Ay），終點坐标為（Bx，By），那麼曼哈頓距離為
這樣一來，目前點到起點的距離就不是唯一評判最優的标準，同時加入父節點的概念用于回溯，是以在A*算法中，F才是最優的度量，父節點用于從終點找到這條最優的路徑。
在上式中，如果忽略H，那麼A*算法就變成了Dijsktra算法，此時保證精度但不保證速度；如果忽略G，則變成BFS，此時保證速度但不保證精度。實際中，我們一般改變的是H的設定，是以也可以說，H設定的大就偏向速度，H設定小就偏向于精度。
下面看一個例子：如下圖所示，綠色方塊為機器人起始位置A，紅色方塊為目标位置B，藍色為障礙物。
現用A*算法尋找出一條自A到B的最短路徑，每個方格的邊長為10，即垂直水準方向移動開銷為10。是以沿對角移動開銷約等于14。具體步驟如下：
從起點 A 開始，把它加入到一個由方格組成的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值最小的那個方格。H值通過估算起點到終點 ( 紅色方格 ) 的 Manhattan 距離得到。這種方式下計算H，起點右邊的方格到終點有3 個方格的距離，是以 H = 30 。這個方格上方的方格到終點有 4 個方格的距離 ( 注意隻計算橫向和縱向距離 ) ，是以 H = 40 。
比較open list中節點的F值後，發現起點A右側節點的F=40，值最小。選作目前處理節點，并将這個點從Open List删除，移到Close List中，表示這個節點是我們探索分析過的了。
對這個節點周圍的8個格子進行判斷，若是某一個格子不可通過（或已經在Close List中，則忽略。否則執行以下步驟：
若目前處理節點的相鄰格子已經在Open List中，則檢查這條路徑是否更優，即計算經由目前處理節點到達那個方格是否具有更小的 G值（其實是比較F，由于H不會變，是以比較G就行）。如果沒有，不做任何操作。如果G值更小，則把這個方格的父節點設為目前處理節點 ，然後更新那個方格的 F 值和 G 值。
若目前處理節點的相鄰格子不在Open List中，那麼把它加入，并将它的父節點設定為該節點。
按照上述規則，選擇起點右邊的方格作為目前處理節點。它的外框用藍線打亮，被放入了close list 中。然後我們檢查與它相鄰的方格。它右側的3個方格是牆壁，我們忽略。它左邊的方格是起點在 close list 中，我們也忽略。其他4個相鄰的方格均在 open list 中，我們需要檢查經由目前節點到達那裡的路徑是否更好。當把4個已經在 open list 中的相鄰方格都檢查後，沒有發現經由目前節點的更好路徑，是以不做任何改變。接下來要選擇下一個待處理的節點。是以再次周遊open list ，選擇F值最小的那個。這次有兩個方格的F值都是54，随機選擇起點右下方的方格作為目前處理節點，如下圖所示。
接下來把起點右下角的方格作為目前處理節點，檢查其相鄰的方格。我們發現它右邊是牆（牆下面的一格也忽略掉，假定牆角不能直接穿越)忽略之。目前方格下面的 2 個方格還沒有加入 open list ，是以把它們加入，同時把目前方格設為他們的父親。目前方格左邊的方格，檢查經由目前方格到達那裡是否具有更小的 G 值。沒有，是以我們從 open list 中選擇下一個待處理的方格。
不斷重複這個過程，直到把終點也加入到了 open list 中。從終點開始，沿着箭頭向父節點移動，直至回到起點，這就是你的路徑。
總結：
1. 把起點加入 open list 。
2. 重複如下過程：
a. 周遊open list ，查找F值最小的節點，把它作為目前要處理的節點，然後移到close list中
b. 對目前方格的 8 個相鄰方格一一進行檢查，如果它是不可抵達的或者它在close list中，忽略它。否則，做如下操作：
如果它不在open list中，把它加入open list，并且把目前方格設定為它的父親
如果它已經在open list中，檢查這條路徑 ( G ) 是否更近。如果更近，把它的父親設定為目前方格，并更新它的G和F值。
c. 遇到下面情況停止搜尋：
把終點加入到了 open list 中，此時路徑已經找到了。
查找終點失敗，open list 是空的，此時沒有路徑。
3. 從終點開始，每個方格沿着父節點移動直至起點，形成路徑。
實作（C++）：
#ifndef CASTARALGORITHM_H
#define CASTARALGORITHM_H
#include <map>
#include <vector>
#include <cmath>
template<typename T> class CAStarAlgorithm {
public:
virtual bool isSolutionEnded(const T &sol)=0;
virtual bool isSolutionValid(const T &sol)=0;
virtual void generateChildren(const T &sol,std::vector<T> &sols)=0;
virtual double getHeuristic(const T &sol)=0;
virtual double getCost(const T &sol)=0;
private:
inline double getTotalCost(const T &sol) {
return getHeuristic(sol)+getCost(sol);
}
public:
int getOptimalSolution(const T &initialSol,T &finalSol,double upperLevel=HUGE_VAL,double maxComputationTime=HUGE_VAL) {
std::multimap<double,T> partialSols;
partialSols.insert(std::pair<double,T>(getTotalCost(initialSol),initialSol));
double currentOptimal=upperLevel;
bool found=false;
std::vector<T> children;
while (!partialSols.empty()) {
typename std::multimap<double,T>::iterator it=partialSols.begin();
double tempCost=it->first;
if (tempCost>=currentOptimal) return found?1:0;
T tempSol=it->second;
partialSols.erase(it);
if (isSolutionEnded(tempSol)) {
currentOptimal=tempCost;
finalSol=tempSol;
found=true;
continue;
}
generateChildren(tempSol,children);
for (typename std::vector<T>::const_iterator it2=children.begin();it2!=children.end();++it2) if (isSolutionValid(*it2)) {
bool alreadyPresent=false;
double cost=getTotalCost(*it2);
typename std::pair<typename std::multimap<double,T>::const_iterator,typename std::multimap<double,T>::const_iterator> range = partialSols.equal_range(cost);
for (typename std::multimap<double,T>::const_iterator it3=range.first;it3!=range.second;++it3) if (it3->second==*it2) {
alreadyPresent=true;
break;
}
if (!alreadyPresent) partialSols.insert(std::pair<double,T>(getTotalCost(*it2),*it2));
}
}
return found?1:0;
}
};
#endif
對于A*算法的改進
1. Breaking ties
導緻低性能的一個原因來自于啟發函數的。當某些路徑具有相同的f值的時候，它們都會被搜尋，盡管我們隻需要搜尋其中的一條：
為了解決這個問題，我們可以為啟發函數添加一個附加值。附加值對于結點必須是确定性的（不能是随機的數），而且它必須讓f值展現差別。
Steven van Dijk建議，一個直截了當的方法是把h傳遞到比較函數。當f值相等時，比較函數檢查h，然後添加附加值。一般傾向于優先搜尋後加入到open例表中節點。
還有一個方法是比較不同節點與起點之間的向量，看哪一個向量與起點到終點的向量方向更貼合，就選擇哪一個節點作為下一個搜尋的節點：
dx1 = current.x - goal.x
dy1 = current.y - goal.y
dx2 = start.x - goal.x
dy2 = start.y - goal.y
cross = abs(dx1*dy2 - dx2*dy1)
heuristic += cross*0.001
這段代碼計算初始-目标向量和目前-目标向量的向量叉積。将叉乘積的結果加入H中，進而加入判定。當沒有障礙物時，A*不僅搜尋很少的區域，而且它找到的路徑看起來非常棒：
有障礙物時的表現也還不錯：
2. beam search
在A*的主循環中，OPEN集儲存所有需要檢查的結點。Beam Search是A*算法的一個變種，這種算法限定了OPEN集的尺寸。如果OPEN集變得過大，那些沒有機會通向一條好的路徑的結點将被抛棄。
3.疊代深化
疊代深化是一種在許多AI算法中使用的方法，這種方法從一個近似解開始，逐漸得到更精确的解。該名稱來源于遊戲樹搜尋，需要檢視前面幾步（比如在象棋裡），通過檢視前面更多步來提高樹的深度。一旦你的解不再有更多的改變或者改善，就可以認為你已經得到足夠好的解，當你想要進一步精确化時，它不會再有改善。在A*中，深度是f值的一個cutoff。當f的值太大時，結點甚至将不被考慮（例如，它不會被加入OPEN集中）。第一次疊代隻處理很少的結點。此後每一次疊代，通路的結點都将增加。如果你發現路徑有所改善，那麼就繼續增加cutoff，否則就可以停止了。
4.動态衡量
在動态衡量中，你假設在開始搜尋時，最重要的迅速移動到任意位置；而在搜尋接近結束時，最重要的是精确移動到目标點。是以，開始時注意速度，快結束時注意精度。
f(p) = g(p) + w(p) * h(p)
啟發函數中帶有一個權值（weight）（w>=1）。當你接近目标時，你降低這個權值；這降低了啟發函數的重要性，同時增加了路徑真實代價的相對重要性。
5.雙向搜尋
與從開始點向目标點搜尋不同的是，你也可以并行地進行兩個搜尋——一個從開始點向目标點，另一個從目标點向開始點。當它們相遇時，你将得到一條好的路徑。
雙向搜尋的思想是，搜尋過程生成了一棵在地圖上散開的樹。一棵大樹比兩棵小樹差得多，是以最好是使用兩棵較小的搜尋樹。
面對面的方法（The front-to-front variation）把這兩種搜尋結合在一起。這種算法選擇一對具有最好的g(start,x) + h(x,y) + g(y,goal)的結點，而不是選擇最好的前向搜尋結點——g(start,x) + h(x,goal)，或者最好的後向搜尋結點——g(y,goal) + h(start,y)。
6.帶寬搜尋
帶寬搜尋（Bandwidth Search）假設h是過高估計的值，但不高于某個數e。如果這樣，那麼你得到的路徑的代價将不會比最佳路徑的代價超過e。
你可以丢棄OPEN集中的某些結點。當h比路徑的真實代價高的時候，你可以丢棄那些f值比OPEN集中的最好結點的f值高>e的結點。對于好的f值你有一個“範圍”（"band"），任何在這個範圍之外的結點都可以被丢棄掉，因為這個結點肯定不會在最佳路徑上。
你可以使用不同的啟發函數，使用一個啟發函數以保證你得到的路徑不會太差，另一個用于檢查從OPEN集中去掉哪些結點。