浅谈迪杰斯特拉（Dijkstra）算法和A*算法原理及实现

写在前面

        最近我在学习一门名叫《智能自主机器人及系统》的课程，虽然跟过去所学的《机器人学》在部分内容上有所重复，但该课程的应用性更强。对于不同的机器人，如差速轮式车、四轮车、四旋翼、仿人机器人的建模进行了深入的探讨（如果有机会我会将其总结发布）。 

        最近课程进展到了智能机器人的决策与规划。其中规划中最基础的问题是最短路径搜索问题。这个问题的求解方法在以前的《数据结构与算法》课程中已经学习过，在《运筹学》课程中又有提及。最广为人知的最短路径搜索算法就是迪杰斯特拉（Dijkstra）算法和A*算法（中文读作“A星”，英文为"A star"）。可是当时并没有很深入地去理解，只是懵懵懂懂地知道了“贪心算法”、“启发式函数”什么的，虽然能写出伪代码，但总觉得没有系统性地学习。最近课程中又学习到这两个算法，重新在网上搜集资料，找到关于这两个算法不错的教程，同时我又对“A*算法最优性的条件”进行了推导证明。废话不多说，让我们进入正题。

Dijkstra算法

算法原理

        给定一个图（graph）和图中的一个起点（source vertex），找到从起点到图中每个点的最短距离和路径。在求解的过程中，需要不断更新维护两个集合：一个集合包含已经找到最短路径的点，另一个集合包含未找到最短路径的点。在每次迭代过程中，从未求得最短路径的点中找离起点最近的点。详细的步骤如下：

1. 创建一个布尔数组sptSet(Shortest path tree set)，以记录各点是否已经得到最短路径。对应某个点为True时，说明其已经得到最短路径；反之。初始时该数组全为False。
2. 创建一个数组dist(distance)，以记录各点到起点的距离。初始时所有距离值设为无穷大，将起点的距离设为0，以使他第一个被操作。
3. 当sptSet还未包含所有点时，进行如下的循环：
   1. 从不在sptSet（即sptSet[u] == False）中取出一个具有最小距离的点u。
   2. 将点u放入到sptSet中（即令sptSet[u] = True）。
4. 更新点u相邻点的dist值。对于每个相邻点v，如果从起点到点u再到点v的距离，小于从起点直接到点v的距离，则更新点v的dist值。

        对以上步骤谈一谈自己的一点理解。对3.1中取出的点u，可以将其放入sptSet的原因是：假如当前点u到起点的距离不是最短距离，意味着经过别的点再到点u会比该距离小。对于不在sptSet的点来说，经过它们本身的距离就要大于现在这个距离，不可能再小；对于已经在sptSet的点来说，每轮迭代在这些点进入sptSet时，会进行一次更新（3.3），有考虑过先经过这些点再到点u的距离，当出现更小的时候会更新。所以没有点能再作为“踏板”使这个值再小了。

        下面看一个例子来应用上述的步骤。

        所给图中有9个节点。布尔数组sptSet初始化为空，即{0, 0, ..., 0}；而数组dist初始化为{0, inf, ..., inf}，其中inf表示无穷大。现在从dist中选一个拥有最小值的点，点0。这时sptSet = {1, 0, ..., 0}。然后更新相邻点的dist，点1和点7的距离被更新成4和8，也就是dist[1] = 4, dist[7] = 8。下图中已经在sptSet内的点被标注成绿色。

        再从dist中选一个拥有最小值的点，点1。放入sptSet，此时sptSet = {1, 1, 0, ..., 0}。然后更新dist，点2的距离被更新为12，也就是dist[2] = 12。

        然后再选，点7。令sptSet[7] = 1，然后更新点7的相邻点dist[6] = 9, dist[8] = 15。

        再选点6，sptSet[6] = 1，dist[5] = 11，dist[8] == dist[6] + graph[6][8]，其中graph[6][8]表示从点6到点8的距离。恰好相等，可以选择更新或者不更新（影响父节点）。不断重复，最后得到每一个点的最短距离，而最短路径需要回溯，可以通过新增一个数组，记录每个点的父节点。这个在代码中有实现，回溯过程利用了“栈”这种数据结构。

代码实现

        使用C++实现如下。

#include 
#include 
#include 
using namespace std;

#define V 9  // Number of vertices in the graph

//void printPathTo(int src, int v, int parent[])
//{
//    if (v == src)
//    {
//        cout << "Path: " << src;
//        return;
//    }
//
//    printPathTo(src, parent[v], parent);
//    cout << "->" << v;
//
//    return;
//}

void printPathTo(int src, int dst, int parent[])
{
    stack path;
    int v = dst;
    while (v != src)
    {
        path.push(v);
        v = parent[v];
    }

    cout << src;
    
    while (!path.empty())
    {
        int n = path.top();
        path.pop();
        cout << "->" << n;
    }
    
    cout << endl;
    
    return;
}

void printSolution(int dist[], int parent[])
{
    cout << "Vertex \t Distance from Source \t Parent" << endl;
    for (int v = 0; v < V; v++)
        cout << v << " \t\t" << dist[v] << " \t\t" << parent[v] << endl;
}

int minDistance(int dist[], bool sptSet[])
{
    int min = INT_MAX, min_index;

    for (int v = 0; v < V; v++)
        if (sptSet[v] == false && dist[v] <= min)
            min = dist[v], min_index = v;

    return min_index;
}

void dijkstra(int graph[V][V], int src, int dst)
{
    int dist[V];     // Shortest distance from src to v
    bool sptSet[V];  // Whether finalized
    int parent[V];   // Parent of each vertix

    for (int i = 0; i < V; i++)
        dist[i] = INT_MAX, sptSet[i] = false;

    parent[src] = -1;

    dist[src] = 0;

    for (int count = 0; count < V - 1; count++)
    {
        int u = minDistance(dist, sptSet);
        
        sptSet[u] = true;

        for (int v = 0; v < V; v++)
            if (!sptSet[v] && graph[u][v] && dist[u] != INT_MAX 
                && dist[u] + graph[u][v] < dist[v])
                dist[v] = dist[u] + graph[u][v], parent[v] = u;
    }

    printSolution(dist, parent);

    cout << endl;

    printPathTo(src, dst, parent);
}

int main()
{
    int graph[V][V] = { { 0, 4, 0, 0, 0, 0, 0, 8, 0 },
                        { 4, 0, 8, 0, 0, 0, 0, 11, 0 },
                        { 0, 8, 0, 7, 0, 4, 0, 0, 2 },
                        { 0, 0, 7, 0, 9, 14, 0, 0, 0 },
                        { 0, 0, 0, 9, 0, 10, 0, 0, 0 },
                        { 0, 0, 4, 14, 10, 0, 2, 0, 0 },
                        { 0, 0, 0, 0, 0, 2, 0, 1, 6 },
                        { 8, 11, 0, 0, 0, 0, 1, 0, 7 },
                        { 0, 0, 2, 0, 0, 0, 6, 7, 0 } };

    dijkstra(graph, 0, 4);

    return 0;
}

运行结果

  

A*算法

算法原理

        在很多实时地图、游戏等场景，在有障碍物的情况下，要快速得到最短路径。地图可以是如下的二维栅格地图，里面有一些障碍被标为黑色，从起点（红色）出发，寻找一条最短路径到达终点（绿色）。

        A*算法是在路径搜索中用得最广泛，效果较好的一种算法。通俗地讲，A*算法不像别的算法，它是有“脑子”的。它使用到了启发式（Heuristics）函数来帮助搜索更快收敛到最短路径，非常高效。

        A*算法在每次迭代时，根据每个点的f值，f = g + h，其中

• g: 从起点到该点的已经消耗的代价。
• h: 从该点到终点预估的代价，也称为“启发式函数”，后面将详细地介绍不同的计算方法。

        A*算法详细的步骤如下：

1. 初始化open表，用以存放待遍历的点。将起点放入open表。
2. 初始化closed表，用以存放已遍历过的点。
3. 当open表非空时，进行如下的循环：
   1. 从open表中的点选出具有最小f值的点q。
   2. 将点q从open表中取出，放入closed表。
   3. 生成点q的八个相邻点（上下左右，右上右下左下左上），并把它们的父节点设为点q。
   4. 对于每个相邻点n：
      1. 如果点n是终点，停止搜索。n.g = q.g + distance(q, n)；n.h = h(n)。有多种启发式函数，后文将详细说明。
      2. 如果点n已经在open表中，且表中的f(n)较小，跳过该点。
      3. 如果点n已经在closed表中，且表中的f(n)较小，跳过该点。
      4. 其余情况均将点n加入到open表中。

        f = g + h中，g可以通过将父节点的g累积，比如在栅格地图中，子节点的g就是父节点的g加1；而h要怎么计算？有以下两种大的分类：

1. 提前计算该点到终点的实际代价（需要消耗时间）。
2. 用启发式函数估计代价（节省时间）。

        如果选择计算实际代价，则要在运行A*算法前，先对图上两两栅格之间进行计算，求出实际代价；假如没有障碍物，则可以认为代价就是欧氏距离。

        如果选择预估代价，则有以下三种常用的估计函数：

    1. 曼哈顿距离（Manhattan Distance）

 h = abs (current_cell.x – goal.x) + abs (current_cell.y – goal.y)

        就是计算当前点和终点横纵坐标的差的绝对值之和。通俗地讲就是每一步只能横着走或竖着走1格，需要走多少步。曼哈顿是美国纽约的中心区，摩天大楼整齐、密集地排列，想象你开着一辆车，俯瞰发现，在道路上只能沿东西方向或南北方向行驶，两地的距离该怎么计算。

    2. 对角线距离（Diagonal Distance）

 

dx = abs(current_cell.x – goal.x)
dy = abs(current_cell.y – goal.y)
 
h = D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)

where D is length of each node(usually = 1) and D2 is diagonal distance between each node (usually = sqrt(2) ). 

        当允许斜向运动时，走对角线的代价应该和走直线的代价相同。所以只用求出当前点与终点之间的横纵坐标之差绝对值最大值即为运动的代价。两点间最短的走法应该是尽可能走对角线，直到某一坐标跟终点对齐了，再沿这个坐标轴直线前进。仔细看这个计算公式，考虑到了点之间的直线距离和对角线距离。

    3. 欧几里得距离（Euclidean Distance）

 h = sqrt ( (current_cell.x – goal.x)2 + (current_cell.y – goal.y)2 )

        就是计算两点间的几何距离。

代码实现

        使用C++实现如下。

// A C++ Program to implement A* Search Algorithm
#include "math.h"
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;

// Creating a shortcut for int, int pair type
typedef pair Pair;
// Creating a shortcut for tuple type
typedef tuple Tuple;

// A structure to hold the necessary parameters
struct cell {
    // Row and Column index of its parent
    Pair parent;
    // f = g + h
    double f, g, h;
    cell()
        : parent(-1, -1)
        , f(-1)
        , g(-1)
        , h(-1)
    {
    }
};

// A Utility Function to check whether given cell (row, col)
// is a valid cell or not.
template 
bool isValid(const array, ROW>& grid, const Pair& point)
{
    if (ROW > 0 && COL > 0)
        return (point.first >= 0) && (point.first < ROW) && (point.second >= 0) && (point.second < COL);

    return false;
}

// A Utility Function to check whether the given cell is
// blocked or not
template 
bool isUnBlocked(const array, ROW>& grid, const Pair& point)
{
    return isValid(grid, point) && grid[point.first][point.second] == 1;
}

// A Utility Function to check whether destination cell has
// been reached or not
bool isDestination(const Pair& position, const Pair& dst)
{
    return position == dst;
}

// A Utility Function to calculate the 'h' heuristics.
double calculateHValue(const Pair& src, const Pair& dst)
{
    // h is estimated with the two points distance formula
    return sqrt(pow((src.first - dst.first), 2.0) + pow((src.second - dst.second), 2.0));
}

// A Utility Function to trace the path from the source to
// destination
template 
void tracePath(const array, ROW>& cellDetails, const Pair& dst)
{
    printf("\nThe Path is ");

    stack Path;

    int row = dst.first;
    int col = dst.second;

    Pair next_node(row, col);
    do 
    {
        Path.push(next_node);
        next_node = cellDetails[row][col].parent;
        row = next_node.first;
        col = next_node.second;
    } while (cellDetails[row][col].parent != next_node);

    Path.emplace(row, col);
    while (!Path.empty()) 
    {
        Pair p = Path.top();
        Path.pop();
        printf("-> (%d,%d) ", p.first, p.second);
    }
}

// A Function to find the shortest path between a given
// source cell to a destination cell according to A* Search
// Algorithm
template 
void aStarSearch(const array, ROW>& grid, const Pair& src, const Pair& dst)
{
    if (!isValid(grid, src) || !isValid(grid, dst))
    {
        printf("Source or destination is invalid\n");
        return;
    }

    if (!isUnBlocked(grid, src) || !isUnBlocked(grid, dst)) 
    {
        printf("Source or the destination is blocked\n");
        return;
    }

    if (isDestination(src, dst)) 
    {
        printf("We are already at the destination\n");
        return;
    }

    // Create a closed list and initialise it to false which
    // means that no cell has been included yet This closed
    // list is implemented as a boolean 2D array
    bool closedList[ROW][COL];
    memset(closedList, false, sizeof(closedList));

    // Declare a 2D array of structure to hold the details of that cell, inited as -1
    array, ROW> cellDetails;

    int i, j;
    // Initialising the parameters of the starting node
    i = src.first, j = src.second;
    cellDetails[i][j].f = 0.0;
    cellDetails[i][j].g = 0.0;
    cellDetails[i][j].h = 0.0;
    cellDetails[i][j].parent = { i, j };

    /*
    Create an open list having information as-
    >
    where f = g + h,
    and i, j are the row and column index of that cell
    Note that 0 <= i <= ROW-1 & 0 <= j <= COL-1
    This open list is implenented as a set of tuple.*/
    std::priority_queue, greater> openList;

    // Put the starting cell on the open list and set its
    // 'f' as 0
    openList.emplace(0.0, i, j);

    // We set this boolean value as false as initially
    // the destination is not reached.
    while (!openList.empty()) 
    {
        const Tuple& p = openList.top();
        // Add this vertex to the closed list
        i = get<1>(p); // second element of tupla
        j = get<2>(p); // third element of tupla

        // Remove this vertex from the open list
        openList.pop();
        closedList[i][j] = true;
        /*
                Generating all the 8 successor of this cell
                     N.W  N  N.E
                       \  |  /
                        \ | /
                    W----Cell----E
                        / | \
                       /  |  \
                     S.W  S  S.E

                Cell-->Popped Cell (i, j)
                N --> North	 (i-1, j)
                S --> South	 (i+1, j)
                E --> East	 (i, j+1)
                W --> West		 (i, j-1)
                N.E--> North-East (i-1, j+1)
                N.W--> North-West (i-1, j-1)
                S.E--> South-East (i+1, j+1)
                S.W--> South-West (i+1, j-1)
        */
        for (int add_x = -1; add_x <= 1; add_x++) {
            for (int add_y = -1; add_y <= 1; add_y++) {
                Pair neighbour(i + add_x, j + add_y);
                // Only process this cell if this is a valid
                // one
                if (isValid(grid, neighbour)) {
                    if (isDestination(neighbour, dst)) 
                    {
                        cellDetails[neighbour.first][neighbour.second].parent = { i, j };
                        printf("The destination cell is found\n");
                        tracePath(cellDetails, dst);
                        return;
                    }
                    else if (!closedList[neighbour.first][neighbour.second] && isUnBlocked(grid, neighbour)) 
                    {
                        double gNew, hNew, fNew;
                        gNew = cellDetails[i][j].g + 1.0;
                        hNew = calculateHValue(neighbour, dst);
                        fNew = gNew + hNew;

                        // If it isn’t on the open list, add
                        // it to the open list. Make the
                        // current square the parent of this
                        // square. Record the f, g, and h
                        // costs of the square cell
                        //			 OR
                        // If it is on the open list
                        // already, check to see if this
                        // path to that square is better,
                        // using 'f' cost as the measure.
                        if (cellDetails[neighbour.first][neighbour.second].f == -1 || 
                            cellDetails[neighbour.first][neighbour.second].f > fNew) 
                        {
                            openList.emplace(fNew, neighbour.first, neighbour.second);

                            // Update the details of this
                            // cell
                            cellDetails[neighbour.first][neighbour.second].g = gNew;
                            cellDetails[neighbour.first][neighbour.second].h = hNew;
                            cellDetails[neighbour.first][neighbour.second].f = fNew;
                            cellDetails[neighbour.first][neighbour.second].parent = { i, j };
                        }
                    }
                }
            }
        }
    }

    printf("Failed to find the Destination Cell\n");
}

int main()
{
    array, 9> grid{
        { { 1, 0, 1, 1, 1, 1, 0, 1, 1, 1 },
          { 1, 1, 1, 0, 1, 1, 1, 0, 1, 1 },
          { 1, 1, 1, 0, 1, 1, 0, 1, 0, 1 },
          { 0, 0, 1, 0, 1, 0, 0, 0, 0, 1 },
          { 1, 1, 1, 0, 1, 1, 1, 0, 1, 0 },
          { 1, 0, 1, 1, 1, 1, 0, 1, 0, 0 },
          { 1, 0, 0, 0, 0, 1, 0, 0, 0, 1 },
          { 1, 0, 1, 1, 1, 1, 0, 1, 1, 1 },
          { 1, 1, 1, 0, 0, 0, 1, 0, 0, 1 } }
    };

    Pair src(8, 0);
    Pair dst(8, 9);

    aStarSearch(grid, src, dst);

    return 0;
}

运行结果

The destination cell is found

The Path is -> (8,0) -> (8,1) -> (8,2) -> (7,3) -> (7,4) -> (7,5) -> (8,6) -> (7,7) -> (7,8) -> (8,9)

一些思考

深度优先搜索与广度优先搜索的区别

        深度优先搜索算法：不全部保留结点，占用空间少；有回溯操作（即有入栈、出栈操作），运行速度慢。

        广度优先搜索算法：保留全部结点，占用空间大；无回溯操作（即无入栈、出栈操作），运行速度快。

Dijkstra和A*算法归于广度优先还是深度优先搜索

        首先我认为都不属于。

        两种算法都是在迭代过程中从未遍历节点中选择拥有最小目标函数值（对于Dijkstra，从起点到该节点的距离）的节点作为下一个操作的对象，我可以将其称为“最小目标函数值”优先搜索。而广度和深度优先搜索一般用于描述图遍历。

        当然也有特例，当图上的边的权重都为1的时候，可以发现Dijkstra和广度优先算法的遍历顺序是一样的。因为越靠近起点，“目标函数值”越小，越先被遍历，层层递进直至终点。

Dijkstra和A*找到的解是否最优解

        对于Dijkstra算法，每一步得到的都是一个节点到起点的最短距离，整个图遍历后终点对应的也是最短距离，没有问题。

        而关于A*算法的最优性就很有意思了。网上的资料很多都说A*算法能找到最优解，但又有说A*算法的最优性取决于启发函数，符合一些条件才能找到最优解，极端例子是启发函数诱导路径走上面，然而最短路径在下面。然而网上又认为，启发式函数不符合条件的A*算法不叫A*算法，是错误的A*算法，而正确的A*算法能确保最优解。

A*算法最优性问题的提出及证明

        参考资料3. Heuristics (stanford.edu) 中提到：

• If h(n) is always lower than (or equal to) the cost of moving from n to the goal, then A* is guaranteed to find a shortest path. The lower h(n) is, the more node A* expands, making it slower.

        引发了我的思考，为什么

就能保证找到的路径是最短路径？这里我参考了许多的资料，其中有一个是这么证明的：

        以上证明步骤，应该将5、6调换顺序看比较通顺。因为假设了g(s)是次优的，所以一定有一个更优的状态s'在open表中，于是有6的推断。但是由于选择的是s而不是s'，所以又有5的推断，两者相互矛盾，所以g(s)一定是最优的。

        如果上述证明还是难以理解的话，不妨来听听我的理解。（如果已经理解了上面的证明，需要谨防以下的内容将你绕晕）

        A*算法使用

作为评估函数，对图上任意点n，h(n)为估计后续代价，g(n)为已产生代价。

        另定义h*(n)和g*(n)分别为h(n)和g(n)的实际值，并具有条件h(n) ≤ h*(n)，也就是保证A*算法最优性的条件。证明如下：

        对于一条搜索出的路径，从起点s到终点t，此时

        假设这条路径不是实际最短路径，则有

        在open表中存在点n是起点s到终点t实际最短路径上的一点，但没有包含在搜索出的路径中，则有g(n) = g*(n)。

        所以

即

        所以f(n) ≤ f*(t) ≤ f(t)，所以应该先处理点n，后处理点t，搜索路径中应该包含点n，但这与前面的条件矛盾。所以假设不成立，搜索的路径是实际最短路径。

参考资料

1. Dijkstra's algorithm - GeeksforGeeks
2. A* Search Algorithm - GeeksforGeeks
3. Heuristics (stanford.edu)