浅谈迪杰斯特拉（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 <iostream>
#include <limits.h>
#include <stack>
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<int> 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)