寻路算法 | GochenRyan的博客

主要介绍了Dijkstra、BFS和A*三种算法。

Dijkstra

Dijkstra算法是典型的单源最短路径算法，适用于求解没有负权边的带权有向图的单源最短路径问题，所谓的单源可以理解为一个出发点，Dijkstra算法可以求得从这个出发点到图中其他顶点的最短路径。

概念

dist[]：存放当前找到的从s到每个顶点vi的最短路径；
W(s,m)：连接s和m的边的权；
集合S、集合Q：集合S存放所有已知dist[vi]都已经是最短路径的顶点，其余的顶点都在集合Q中；
prev[]：存放当前节点在最短路径上的前驱节点。

算法步骤

初始化集合S和Q，设起点dist[s]=0，并将其他顶点的dist[vi]设为无穷大；
从Q中选择一个dist[u]值最小的顶点u，将u从集合Q中移到集合S中；
以u为当前顶点，修改Q中与u相连的顶点的距离。修改的方法是：对于集合Q中每一个与u相连的顶点vi，如果从起点s经u到的vi距离dist[u] + W(u, vi)的值小于当前vi的距离dist[vi]，则将dist[vi]的值修正为dist[u] + W(u, vi)的值，同时将顶点vi的前驱顶点记为u；
重复步骤(2)和(3)，直到集合Q为空。第(3)步记录顶点的前驱顶点操作不是算法的必需内容，记录前驱顶点的目的仅仅是为了能够回溯每条最短路径的顶点连接关系。

代码实现

void Dijkstra(DIJKSTRA_GRAPH graph){
    std::set<int> S,Q;
    for(int i = 0; i < graph->node.size(); i++) {
        graph->dist[i] = graph->[graph->source][i];
        graph->prev[i] = (graph->dist[i] == MAX_DISTANCE)? -1:graph->source;
        Q.insert(i);
    }
    graph->dist[graph->source] = 0;
    while(!Q.empty()){
        int u = Extract_Min(graph,Q);
        S.insert(u);
        for(auto it = Q.begin(); it != Q.end(); ++it) { 
             int v = it;
             if((graph->adj[u][v] <MAX_DISTANCE)&&(graph->dist[u] + graph->adj[u][v] <graph->dist[v])){ //小于MAX_DISTANCE表示有边相连 
                graph->dist[v] = graph->dist[u] + graph->adj[u][v]; //更新
                distgraph->prev[v] = u; //记录前驱顶点 
             } 
        }
    }
}

启发式搜索算法

BFS

BFS算法是在广度优先搜索算法的基础上加入评估函数，通过评估函数剪枝，避免一些明显不可能得到最短路径的搜索动作。在没有障碍物的地图上，其算法效果接近A*算法，总体上远远优于Dijkstra算法。但是BFS算法因为评价函数只考虑位置方向信息，基于贪心策略，总是试图向最接近目标点的方向移动，使得这种算法在有障碍物的地图上表现不佳，很多情况下得到的路径都不是最短路径。

概念

启发函数：即评估函数，评估函数的作用就是根据起点和终点的位置和距离信息给出下一步需要搜索各个位置的评估值，启发式搜索算法可以有以下三种方式利用这些评估值：

根据评估结果，每次选择评估值最高的位置开始下一步搜索，避免盲目的穷举搜索；
决定搜索的顺序，按照评估值的高低排序，从评估值最高的位置开始下一步搜索（如果评估值高的位置没找到结果，则评估值较低的位置也能被顺序处理到）；
剪枝，去除一些明显不可能得到最优结果的搜索位置，提高搜索效率。

A*

A*算法既能像Dijkstra算法那样搜索到最短路径，又能像BFS算法一样使用启发函数进行启发式搜索，是目前各种寻径算法中最受欢迎的选择。即使在有障碍物的情况下，选择合适的距离评估函数，A*算法基本上都能搜索到最短路径。
A*算法的启发函数采用的计算公式是：F(n) = G(h) + H(n)。F(n)就是A*算法对每个点的评估函数。

概念

G(n)：从起点到当前节点n的实际代价，也就是从起点到当前节点的移动距离。相邻的两个点的移动距离是1，当前点距离起点越远，这个值就越大。如果我们设G(n)的值总是0，则算法的效果类似于BFS算法；
H(n)：从当前节点n到终点的距离评估值。这是一个从当前节点到终点的移动距离的估计值。如果设H(n)的值总是0，则算法可退化得到类似Dijkstra算法的效果；
OPEN表：存放当前已经被发现但是还没有搜索过的节点；
CLOSE表：存放已经搜索过的节点；
距离评估函数：曼哈顿距离、欧氏几何平面距离和切比雪夫距离；
曼哈顿距离：两个点在各个坐标轴上的距离差值的几何；
欧式几何平面距离：n维空间中两个点之间的真实距离（几何距离）；
切比雪夫距离：向量中各个分量的差的绝对值中最大的那一个。

算法步骤

初始化OPEN表和CLOSE表，将起点加入到OPEN表中；
从OPEN表中取出当前F(n)值最小的节点作为当前搜索节点U，将U节点加入到CLOSE表中；
对于每一个与U可连通的节点（障碍物不相通）V，考察V：如果V已经在CLOSE表中，则对该节点不做任何处理；如果V不在OPEN表中，则计算F(V)，将V的前驱节点设置为U并将V加入到OPEN表中；如果V在OPEN表中，比较G(U) + 1与G(V)的大小（H(V)的值是不变的），如果G(U) + 1<G(V)，则令G(V) = G(U) + 1，同时将V的前驱节点设置为U。重复步骤(2)和(3)，直到第(2)步得到的搜索节点U就是终点为止，此时算法结束。

代码实现

void AStar(ASTAR_GRAPH graph, GRID_CELL gc) { 
    //步骤(1) 
    graph->open.insert(graph->source); 
    //步骤(2) 
    ANODE cur_node; 
    while(ExtractMiniFromOpen(graph, cur_node)) { 
        graph->close.push_back(cur_node); 
        if(cur_node== graph->target) { 
            UpdateCellInfo(graph, gc); 
            break; 
        } 
        //步骤(3) 
        for(int d = 0; d <COUNT_OF(dir); d++) { 
            ANODEnn = {cur_node.i + dir[d].y, cur_node.j + dir[d].x, 0, 0}; 
            if((nn.i >=0) &&(nn.i <N_SCALE) &&(nn.j >=0) &&(nn.j<N_SCALE) 
            &&(gc->cell[nn.i][nn.j].type != CELL_WALL) &&!IsNodeExistInClose(graph->close, nn.i, nn.j)){ 
                std::multiset<ANODE, compare>::iterator it; 
                it = find(graph->open.begin(), graph->open.end(), nn); 
                if(it ==graph->open.end()){  /*nn不在open列表*/ 
                    nn.g = cur_node.g + 1; 
                    //将g始终赋值为可得到BFS算法的效果 
                    nn.h =ManhattanDistance(nn, graph->target); 
                    nn.prev_i = cur_node.i; nn.prev_j = cur_node.j; 
                    graph->open.insert(nn);gc->cell[nn.i][nn.j].processed = true; 
                } else { /*nn在open列表中*/ 
                    if((cur_node.g + 1.0) <it->g) { 
                        it->g =cur_node.g + 1.0; 
                        it->prev_i = cur_node.i; it->prev_j = cur_node.j; 
                    } 
                } 
            } 
        } 
    } 
}