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集中去掉哪些结点。