数学建模6——路径规划的各种算法(Dijkstra、Floyd、A*、D*、RRT*、LPA*)
前言：本文只是简单的介绍一下各路径规划算法的概念和流程，可用于对算法的初步了解，如果要进一步学习，可以在个人理解中找到我推荐的其他博主更为完善的文章。
目录
一、Dijkstra
基本概念
基本流程
个人理解
MATLAB代码
二、Floyd
基本概念
基本流程
个人理解
MATLAB代码
三、A*算法
基本概念
基本流程
个人理解
MATLAB代码
四、D*算法
基本概念
基本流程
个人理解
MATLAB代码
五、RRT*算法
基本概念
基本流程
个人理解
六、LPA*算法
基本概念
基本流程
个人理解
七、D*lite算法
基本概念
基本流程
个人理解
八、各路径规划算法之间的区别（重要）
最后总结
一、Dijkstra
基本概念
Dijkstra算法是一种用于求解图中单源最短路径问题的经典算法。它可以用来找到从一个顶点到其他所有顶点的最短路径。
以下是Dijkstra算法的基本概念：
数据结构：Dijkstra算法使用两个主要的数据结构来实现最短路径的计算（就是两个数据集）：
顶点集合：包含图中所有的顶点。
距离集合：记录从起点到每个顶点的最短路径距离。
初始化：首先，将起点的距离设置为0，其他顶点的距离设置为无穷大。将起点设置为当前顶点。
迭代更新：重复以下步骤，直到所有顶点都被标记为已访问。
选择当前顶点的邻居中距离最短的顶点，设为下一个当前顶点。
更新其他邻居的距离，如果通过当前顶点到达邻居的距离比当前记录的距离要短，则更新距离。
标记顶点：在每次迭代更新后，将当前顶点标记为已访问，表示已经找到了从起点到该顶点的最短路径。
最短路径提取：在所有顶点都被标记为已访问后，就可以提取出从起点到其他所有顶点的最短路径。
Dijkstra算法的基本思想是通过不断更新距离集合中的距离，逐步找到最短路径。它保证每次迭代都会选择当前距离最短的顶点作为下一个当前顶点，并通过该顶点更新其他顶点的距离。最终，得到起点到其他所有顶点的最短路径和对应的距离。
基本流程
Dijkstra算法的基本流程如下：
创建一个距离集合和一个顶点集合。距离集合用于记录从起点到每个顶点的最短路径距离，初始时所有距离设置为无穷大（表示无法到达）。顶点集合用于存储图中的所有顶点。
将起点的距离设置为0，并将其标记为当前顶点。
迭代更新距离集合，直到所有顶点都被标记为已访问。 a. 遍历当前顶点的所有邻居顶点。 b. 对于每个邻居顶点，计算通过当前顶点到达该邻居顶点的距离，即当前顶点的距离加上从当前顶点到邻居顶点的边的权重。 c. 如果计算得到的距离小于距离集合中记录的距离，则更新距离集合中该邻居顶点的距离。 d. 重复步骤a到c，直到遍历完当前顶点的所有邻居顶点。
将当前顶点标记为已访问，表示已经找到了从起点到该顶点的最短路径。
选择下一个当前顶点：从未访问的顶点中选择距离最短的顶点作为下一个当前顶点。
重复步骤3到5，直到所有顶点都被标记为已访问。
最短路径提取：根据距离集合中记录的最短路径距离，可以回溯找到从起点到其他所有顶点的最短路径。
通过以上流程，Dijkstra算法能够逐步更新距离集合中的距离，最终得到从起点到其他所有顶点的最短路径和对应的距离。
个人理解
说白了就是问你：上面的图你怎么走可以把所有的点都走一遍，然后还需要路径长度最短。
这方法给出的答案是：我一个个点去摸索，比如我先从0到2最近，然后再从2开始摸索，发现接下来走1最好，因为0-2-1是5米，但0-2-3是8，0-2-5是10，然后我们确定0-2-1之后，再从1开始摸索，就这样慢慢找，最后找到最短路径。
具体学习可以参考这篇文章：图算法——求最短路径（Dijkstra算法）
MATLAB代码
%在这个代码中，首先定义了一个带权有向图的邻接矩阵，然后定义了起点和终点。接着，初始化到各个节点的距离、前驱节点和已访问状态。然后，运行Dijkstra算法，找到最短路径和最短距离。最后，使用MATLAB的fprintf函数输出结果。
% 定义带权有向图
n = 5; % 节点数
adj_matrix = [0 10 0 5 0; 0 0 1 2 0; 0 0 0 0 4; 0 3 9 0 2; 7 0 6 0 0]; % 邻接矩阵
% 初始化
start_node = 1; % 起点
end_node = 5; % 终点
dist = inf(1, n); % 到各个节点的距离
prev = zeros(1, n); % 前驱节点
visited = zeros(1, n); % 是否已访问
dist(start_node) = 0;
% 运行Dijkstra算法
for i = 1:n
% 找到最近的未访问节点
min_dist = inf;
for j = 1:n
if visited(j) == 0 && dist(j) < min_dist
min_dist = dist(j);
curr_node = j;
end
end
% 更新距离和前驱节点
visited(curr_node) = 1;
for j = 1:n
if adj_matrix(curr_node, j) > 0 && visited(j) == 0
new_dist = dist(curr_node) + adj_matrix(curr_node, j);
if new_dist < dist(j)
dist(j) = new_dist;
prev(j) = curr_node;
end
end
end
end
% 输出结果
path = [end_node];
while path(1) ~= start_node
path = [prev(path(1)), path];
end
fprintf('最短路径：');
fprintf('%d ', path);
fprintf('\n最短距离：%d\n', dist(end_node));
二、Floyd
Floyd算法又称为Floyd-Warshall算法，是一种动态规划算法，用于解决任意两点之间的最短路径问题。该算法在图中存在负权边和环的情况下仍然保证正确性。
Floyd算法的思想是动态规划，通过枚举所有可能的中间节点，来逐步更新每个节点之间的距离。具体而言，算法根据当前结点之间的最短路径和中间节点更新后的最短路径，依次更新每个节点之间的最短路径。由于每次先将中间节点作为当前节点进行考虑，再按照不同的中间节点进行循环，所以也被称为多源最短路径算法。
基本概念
初始化：
对于每对节点之间的边，设置其距离为边上的权值。
对于不存在直接相连的节点，其距离设置为无穷大（或者一个较大的数）。
对角线上的元素距离均为0。
枚举中间节点：
对于图中的每个节点，假设其为中间节点。即以该节点为“中转站”，更新每对节点之间的距离。
对于每一对节点i,j，若它们的距离经过中间节点k比原先更优，则更新它们之间的距离为经过节点k的距离。
重复迭代，直到所有节点之间的最短路径都求出：
如果所有节点都已经被遍历，算法终止。
否则，返回第2步，继续选择下一个中间节点进行更新。
最短路径获取：
当所有节点之间的最短路径都求出后，就可以根据Floyd算法矩阵中的数据来获取起始节点到每个节点的最短路径。
Floyd算法的时间复杂度为O(n^3)，虽然有一定的计算量，但该算法相对于其他单源最短路径算法处理任意两点之间的最短路径问题十分高效。
基本流程
创建一个二维数组D，用于存储任意两点之间的最短路径长度。初始时，D[i][j]表示顶点i到顶点j的边的权重，如果i和j之间没有边相连，则D[i][j]为无穷大。
通过枚举中间节点k，逐步更新D数组中的元素。即，对于所有的i和j，若D[i][j] > D[i][k] + D[k][j]，则更新D[i][j]为D[i][k] + D[k][j]。
重复步骤2，直到枚举完所有的中间节点k。
最终得到的D数组即为任意两点之间的最短路径长度。
值得注意的是，在Floyd算法中，对于每个中间节点k，都会更新一次所有顶点之间的距离，时间复杂度为O(n^3)。因此，Floyd算法适用于顶点数较少的图，对于大型图来说效率较低。
个人理解
还是这个图，只不过这个算法更容易找的是任意两点之间的最短距离，我们需要先弄个2维数组，然后一个点一个点的去修改权重，最后得出结果，不过这个算法可以解决负权边的情况，这也是他的优点。（但是是真的蛮麻烦的，如果顶点多要累死）