浅谈迪杰斯特拉（Dijkstra）算法和A*算法原理及实现
写在前面
最近我在学习一门名叫《智能自主机器人及系统》的课程，虽然跟过去所学的《机器人学》在部分内容上有所重复，但该课程的应用性更强。对于不同的机器人，如差速轮式车、四轮车、四旋翼、仿人机器人的建模进行了深入的探讨（如果有机会我会将其总结发布）。
最近课程进展到了智能机器人的决策与规划。其中规划中最基础的问题是最短路径搜索问题。这个问题的求解方法在以前的《数据结构与算法》课程中已经学习过，在《运筹学》课程中又有提及。最广为人知的最短路径搜索算法就是迪杰斯特拉（Dijkstra）算法和A*算法（中文读作“A星”，英文为"A star"）。可是当时并没有很深入地去理解，只是懵懵懂懂地知道了“贪心算法”、“启发式函数”什么的，虽然能写出伪代码，但总觉得没有系统性地学习。最近课程中又学习到这两个算法，重新在网上搜集资料，找到关于这两个算法不错的教程，同时我又对“A*算法最优性的条件”进行了推导证明。废话不多说，让我们进入正题。
Dijkstra算法
算法原理
给定一个图（graph）和图中的一个起点（source vertex），找到从起点到图中每个点的最短距离和路径。在求解的过程中，需要不断更新维护两个集合：一个集合包含已经找到最短路径的点，另一个集合包含未找到最短路径的点。在每次迭代过程中，从未求得最短路径的点中找离起点最近的点。详细的步骤如下：
- 创建一个布尔数组sptSet(Shortest path tree set)，以记录各点是否已经得到最短路径。对应某个点为True时，说明其已经得到最短路径；反之。初始时该数组全为False。
- 创建一个数组dist(distance)，以记录各点到起点的距离。初始时所有距离值设为无穷大，将起点的距离设为0，以使他第一个被操作。
- 当sptSet还未包含所有点时，进行如下的循环：
- 从不在sptSet（即sptSet[u] == False）中取出一个具有最小距离的点u。
- 将点u放入到sptSet中（即令sptSet[u] = True）。
- 更新点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)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 vbool sptSet[V]; // Whether finalizedint parent[V]; // Parent of each vertixfor (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*算法详细的步骤如下：
- 初始化open表，用以存放待遍历的点。将起点放入open表。
- 初始化closed表，用以存放已遍历过的点。
- 当open表非空时，进行如下的循环：
- 从open表中的点选出具有最小f值的点q。
- 将点q从open表中取出，放入closed表。
- 生成点q的八个相邻点（上下左右，右上右下左下左上），并把它们的父节点设为点q。
- 对于每个相邻点n：
- 如果点n是终点，停止搜索。n.g = q.g + distance(q, n)；n.h = h(n)。有多种启发式函数，后文将详细说明。
- 如果点n已经在open表中，且表中的f(n)较小，跳过该点。
- 如果点n已经在closed表中，且表中的f(n)较小，跳过该点。
- 其余情况均将点n加入到open表中。
f = g + h中，g可以通过将父节点的g累积，比如在栅格地图中，子节点的g就是父节点的g加1；而h要怎么计算？有以下两种大的分类：
- 提前计算该点到终点的实际代价（需要消耗时间）。
- 用启发式函数估计代价（节省时间）。
如果选择计算实际代价，则要在运行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 <array>
#include <chrono>
#include <cstring>
#include <iostream>
#include <queue>
#include <set>
#include <stack>
#include <tuple>
using namespace std;// Creating a shortcut for int, int pair type
typedef pair<int, int> Pair;
// Creating a shortcut for tuple<int, int, int> type
typedef tuple<double, int, int> Tuple;// A structure to hold the necessary parameters
struct cell {// Row and Column index of its parentPair parent;// f = g + hdouble 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 <size_t ROW, size_t COL>
bool isValid(const array<array<int, COL>, 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 <size_t ROW, size_t COL>
bool isUnBlocked(const array<array<int, COL>, 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 formulareturn 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 <size_t ROW, size_t COL>
void tracePath(const array<array<cell, COL>, ROW>& cellDetails, const Pair& dst)
{printf("\nThe Path is ");stack<Pair> 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 <size_t ROW, size_t COL>
void aStarSearch(const array<array<int, COL>, 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 arraybool closedList[ROW][COL];memset(closedList, false, sizeof(closedList));// Declare a 2D array of structure to hold the details of that cell, inited as -1array<array<cell, COL>, ROW> cellDetails;int i, j;// Initialising the parameters of the starting nodei = 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-<f, <i, j>>where f = g + h,and i, j are the row and column index of that cellNote that 0 <= i <= ROW-1 & 0 <= j <= COL-1This open list is implenented as a set of tuple.*/std::priority_queue<Tuple, vector<Tuple>, greater<Tuple>> openList;// Put the starting cell on the open list and set its// 'f' as 0openList.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 listi = get<1>(p); // second element of tuplaj = get<2>(p); // third element of tupla// Remove this vertex from the open listopenList.pop();closedList[i][j] = true;/*Generating all the 8 successor of this cellN.W N N.E\ | /\ | /W----Cell----E/ | \/ | \S.W S S.ECell-->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// oneif (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// cellcellDetails[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<array<int, 10>, 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 foundThe 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，但这与前面的条件矛盾。所以假设不成立，搜索的路径是实际最短路径。
参考资料
- Dijkstra's algorithm - GeeksforGeeks
- A* Search Algorithm - GeeksforGeeks
- Heuristics (stanford.edu)
浅谈迪杰斯特拉（Dijkstra）算法和A*算法原理及实现相关推荐
- 059.迪杰斯特拉(Dijkstra)算法的原理以及解决最短路径问题
1. 迪杰斯特拉(Dijkstra)算法的原理 1.1. 算法应用场景-最短路径问题 1.2. 基本介绍 1.3. 步骤详解 1.4. 思路解析 1.5. 图解步骤 2. 迪杰斯特拉(Dijkstra ...
- 数据结构与算法（7-4）最短路径（迪杰斯特拉(Dijkstra)算法、弗洛伊德(Floyd)算法）
目录 一.最短路径概念 二.迪杰斯特拉(Dijkstra)算法(单源最短路径) 1.原理 2.过程 3.代码 三.弗洛伊德(Floyd)算法(多源最短路径) 1.原理 2.存储 3.遍历 4.代码 参 ...
- java数据结构和算法——迪杰斯特拉(Dijkstra)算法
目录 一.迪杰斯特拉(Dijkstra)算法介绍 二.迪杰斯特拉(Dijkstra)算法过程 三.迪杰斯特拉(Dijkstra)算法--应用场景(最短路径问题) 四.迪杰斯特拉(Dijkstra)算法 ...
- 迪杰斯特拉(Dijkstra)算法解决最短路径问题
Dijkstra 算法介绍 迪杰斯特拉算法(Dijkstra)是由荷兰计算机科学家狄克斯特拉于1959年提出的,因此又叫狄克斯特拉算法.迪杰斯特拉(Dijkstra)算法是最经典的最短路径算法之一,用 ...
- c语言迪杰斯特拉算法求最短路径,迪杰斯特拉 ( Dijkstra ) 最短路径算法
迪杰斯特拉算法介绍 迪杰斯特拉(Dijkstra)算法是典型最短路径算法,用于计算一个节点到其他节点的最短路径.它的主要特点是以起始点为中心向外层层扩展(广度优先搜索思想),直到扩展到终点为止. 基本 ...
- 最短路径算法-迪杰斯特拉(Dijkstra)算法
最短路径算法-迪杰斯特拉(Dijkstra)算法 迪杰斯特拉(Dijkstra)算法是典型最短路径算法,用于计算一个节点到其他节点的最短路径. 它的主要特点是以起始点为中心向外层层扩展(广度优先遍历思 ...
- Java迪杰斯特拉(Dijkstra)算法与弗洛伊德(Floyd)算法
1.Java迪杰斯特拉(Dijkstra)算法与弗洛伊德(Floyd)算法 1.1 迪杰斯特拉(Dijkstra)算法 1.1.1 迪杰斯特拉(Dijkstra)算法介绍 迪杰斯特拉(Dijkstra ...
- java实现迪杰斯特拉(Dijkstra)算法求解最短路问题
迪杰斯特拉(Dijkstra)算法是由荷兰计算机科学家狄克斯特拉于1959年提出的.是寻找从一个顶点到其余各顶点的最短路径算法,可用来解决最短路径问题. 迪杰斯特拉算法采用贪心算法的策略,将所有顶点分 ...
- 数据结构——图——迪杰斯特拉(Dijkstra )算法
数据结构--图--迪杰斯特拉(Dijkstra )算法 这是一个按路径长度递增的次序产生最短路径的算法.它的思路大体是这样的. 比如说要求图7-7-3中顶点v0到顶点v1的最短距离,没有比这更简单的了 ...
最新文章
- java 千分位格式话_Java 字符串小数转成千分位格式
- [每日一题] OCP1z0-047 :2013-08-14 如何理解USING INDEX?...................................41
- Ubuntu14.04安装QQ2013
- 解Bug之路-Druid的Bug
- ScriptManager调用 无参数WebService
- 每日一博 - DelayQueue阻塞队列源码解读
- Your local changes would be overwritten by merge. Commit, stash or revert them to proceed.
- 信息学奥赛C++语言：由大到小输出1~100的整数
- 如果一个国家是一种颜色，中国的颜色美哭了！
- [技术文档]Linux从入门到精通
- three.js 来源目光（十三）Math/Ray.js
- ModuleNotFoundError: No module named 'tornado'解决办法
- Vuforia的ARcamera通过识别人工标识码出现虚拟物体后在其相机视野中不显示人工标识码
- koa2 mysql项目教程_blogs: Node.js + Koa2 + MySQL + Vue.js 实战开发一套完整个人博客项目网站...
- html怎么引用网页链接,浅谈网页中各种链接引用方法
- 怎么学大数据？该从哪学起？
- Apifox使用json传参
- 【AI每日播报】马云启动NASA计划 Intel150亿美元收购Mobileye
- 成都榆熙：怎么解除拼多多商品的推广限制
- 一些检索英文文献的网址（收藏）