路径规划算法简介

本文最后更新于：2025年2月26日上午

本文简单介绍各路径规划算法的概念和流程，可用于对算法的初步了解。

Dijkstra

Dijkstra算法是一种用于求解图中单源最短路径问题的经典算法。它可以用来找到从一个顶点到其他所有顶点的最短路径。

基本概念：

数据结构：Dijkstra算法使用两个主要的数据结构来实现最短路径的计算（就是两个数据集）：
1. 顶点集合：包含图中所有的顶点。
2. 距离集合：记录从起点到每个顶点的最短路径距离。
初始化：首先，将起点的距离设置为0，其他顶点的距离设置为无穷大。将起点设置为当前顶点。
迭代更新：重复以下步骤，直到所有顶点都被标记为已访问。
1. 选择当前顶点的邻居中距离最短的顶点，设为下一个当前顶点。
2. 更新其他邻居的距离，如果通过当前顶点到达邻居的距离比当前记录的距离要短，则更新距离。
标记顶点：在每次迭代更新后，将当前顶点标记为已访问，表示已经找到了从起点到该顶点的最短路径。
最短路径提取：在所有顶点都被标记为已访问后，就可以提取出从起点到其他所有顶点的最短路径。
Dijkstra算法的基本思想是通过不断更新距离集合中的距离，逐步找到最短路径。它保证每次迭代都会选择当前距离最短的顶点作为下一个当前顶点，并通过该顶点更新其他顶点的距离。最终，得到起点到其他所有顶点的最短路径和对应的距离。

基本流程

创建一个距离集合和一个顶点集合。距离集合用于记录从起点到每个顶点的最短路径距离，初始时所有距离设置为无穷大（表示无法到达）。顶点集合用于存储图中的所有顶点。
将起点的距离设置为0，并将其标记为当前顶点。
迭代更新距离集合，直到所有顶点都被标记为已访问。

a. 遍历当前顶点的所有邻居顶点。

b. 对于每个邻居顶点，计算通过当前顶点到达该邻居顶点的距离，即当前顶点的距离加上从当前顶点到邻居顶点的边的权重。

c. 如果计算得到的距离小于距离集合中记录的距离，则更新距离集合中该邻居顶点的距离。

d. 重复步骤a到c，直到遍历完当前顶点的所有邻居顶点。
将当前顶点标记为已访问，表示已经找到了从起点到该顶点的最短路径。
选择下一个当前顶点：从未访问的顶点中选择距离最短的顶点作为下一个当前顶点。
重复步骤3到5，直到所有顶点都被标记为已访问。
最短路径提取：根据距离集合中记录的最短路径距离，可以回溯找到从起点到其他所有顶点的最短路径。

通过以上流程，Dijkstra算法能够逐步更新距离集合中的距离，最终得到从起点到其他所有顶点的最短路径和对应的距离。

Floyd

Floyd算法又称为 Floyd-Warshall 算法，是一种动态规划算法，用于解决任意两点之间的最短路径问题。该算法在图中存在负权边和环的情况下仍然保证正确性。

Floyd算法的思想是动态规划，通过枚举所有可能的中间节点，来逐步更新每个节点之间的距离。具体而言，算法根据当前结点之间的最短路径和中间节点更新后的最短路径，依次更新每个节点之间的最短路径。由于每次先将中间节点作为当前节点进行考虑，再按照不同的中间节点进行循环，所以也被称为多源最短路径算法。

基本概念

初始化：
1. 对于每对节点之间的边，设置其距离为边上的权值。
2. 对于不存在直接相连的节点，其距离设置为无穷大（或者一个较大的数）。
3. 对角线上的元素距离均为0。
枚举中间节点：
1. 对于图中的每个节点，假设其为中间节点。即以该节点为“中转站”，更新每对节点之间的距离。
2. 对于每一对节点i,j，若它们的距离经过中间节点k比原先更优，则更新它们之间的距离为经过节点k的距离。
重复迭代，直到所有节点之间的最短路径都求出：
1. 如果所有节点都已经被遍历，算法终止。
2. 否则，返回第2步，继续选择下一个中间节点进行更新。
最短路径获取：
1. 当所有节点之间的最短路径都求出后，就可以根据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算法适用于顶点数较少的图，对于大型图来说效率较低。

A*

A*算法是一种启发式搜索算法，用于在图形或网络中找到最短路径。它结合了Dijkstra算法的广度优先搜索和启发式函数的评估，以提高搜索效率。

介绍链接

D*

D*（D-star）算法是一种增量式路径规划算法，用于在动态环境中更新和重新规划路径。它是 A* 算法的扩展，可以有效地应对环境状态的变化。

基本概念

基于图的路径规划： D* 算法将路径规划问题抽象为一个有向图，图的节点表示位置或状态，图的边表示连接两个节点的可能移动或转换方式。
状态和代价：每个节点都有一个状态和对应的代价值。状态表示节点在环境中的位置或状态信息，代价值表示到达该节点的成本，通常使用距离或时间作为代价。
开放列表和代价图：
1. 开放列表（open list）存储待扩展的节点。
2. 代价图（cost map）存储每个节点的当前代价值。
局部更新： D* 算法通过局部更新操作来快速响应环境的变化。当环境发生变化时，只需要更新与变化相关的节点和边的代价，而不必重新计算整个路径。
反向传播： D* 算法使用反向传播来更新路径和代价信息。通过从目标节点开始反向传播，沿着当前路径反向更新每个节点的代价值，使得路径适应环境变化。
重规划：当环境状态发生变化时，D* 算法会重新规划路径。它从起始节点开始，通过局部更新和反向传播逐步更新路径和代价信息，直到找到一条新的最短路径。

算法是一种基于启发式搜索的路径规划算法，它可以在动态环境中找到最短路径。与A*算法不同，D*算法是增量式的，也就是说，它可以在实时环境中对路径进行增量更新。D*算法最初由Sven Koenig和Maxim Likhachev在2002年提出。

D*算法的基本思想是：在一个有向图上，从起点到终点的最短路径必定是一系列连续的边，而不是一些离散的节点。因此，D算法不是从起点开始，而是从终点开始，不断向起点方向搜索路径，并根据环境的变化动态更新路径。

基本流程

初始化：将终点作为当前节点，将起点的代价f(x)置为0。
扩展节点：从当前节点开始向相邻节点搜索路径，计算路径的代价，更新相邻节点的代价和f(x)值。
路径更新：如果某个节点的代价发生变化，就更新它的相邻节点的f(x)值，并将它们添加到路径列表中。
重复步骤2和3，直到找到起点为止。
D算法的优势是它可以在动态环境中进行实时路径规划。当环境发生变化时，D算法可以从上一次路径的终点处重新开始搜索，避免了重新规划整个路径的开销。但是，D*算法的缺点是它需要在扩展节点时进行路径更新，这可能会导致算法的效率降低。

RRT*

快速搜索随机树

树结构：RRT*使用树结构来表示搜索空间。树的节点表示搜索过程中的状态，边表示节点之间的连接关系。
随机采样：RRT*通过随机采样在搜索空间中生成新的节点。这些采样点可以在整个搜索空间中随机选择，也可以根据目标位置的分布进行更加智能化的采样。
连接节点：通过计算采样点与现有树中最近节点之间的距离，并在两个节点之间创建一条边，将采样点连接到树中。
可达性检查：在连接节点之前，需要对两个节点之间的路径做可达性检查，以确保没有被障碍物或其他限制所阻挡。
近邻节点：为了快速找到最近的节点，RRT*维护一个近邻节点集合。该集合可以根据节点之间的距离进行排序，以便更快地找到最近的节点。
代价函数：RRT*引入了一个代价函数来评估路径的优劣。代价函数考虑了路径的长度和其他性能指标，帮助算法在搜索过程中找到更优的路径。
重连节点：RRT*会定期检查树中的节点，尝试通过改变连接关系来改善现有路径。这样可以在不重新规划整个路径的情况下，通过局部优化来提高路径质量。
目标达到：当搜索树中的节点越来越接近目标位置时，RRT*会终止搜索并返回最优路径。

RRT*算法通过不断扩展树结构和优化路径，使得搜索空间中的采样点趋向于目标，并找到最优路径。它在机器人路径规划、无人机路径规划等领域具有广泛应用。

基本流程

初始化：
创建一个树，将起始位置作为树的根节点。
初始化其他参数，如目标位置、最大迭代次数等。
迭代：
在每次迭代中，生成一个随机采样点（通常是在可行空间内随机选择的点）。
节点选择：
从树中选择最近的节点，该节点将成为新采样点的父节点。
节点扩展：
生成新节点，以使新节点与父节点之间的路径尽可能直接，通常是通过沿直线路径向新采样点前进一定距离。
确保新节点在可行空间内。
连接检查：
检查新节点与父节点之间的路径是否与障碍物碰撞。如果路径与障碍物相交，需要重新选择新节点或采用其他策略。
节点添加：
如果新节点通过了连接检查，则将新节点添加到树中，并建立与父节点的连接。
路径优化：
对树中的节点进行代价优化。这涉及到计算从起始节点到新节点的代价，并与旧节点的代价进行比较。如果新节点的代价更低，就更新路径。
目标检查：
检查新节点是否接近目标位置。如果是，则路径规划完成。
迭代终止条件：
可以设置迭代终止条件，如达到最大迭代次数或规划超时。
路径返回：
如果路径规划成功，从目标节点回溯到起始节点，构建最终路径。
结束：
返回最终路径或报告无法找到路径，根据实际情况采取适当的行动。
请注意，RRT* 的关键特点之一是不断优化节点之间的连接，以寻找最优路径。这意味着随着迭代次数的增加，算法将收敛到全局最优路径（在无障碍情况下），并且具有渐近最优性和一致性的特点。此外，RRT* 可以在高维、复杂的环境中进行路径规划，并且适用于自主机器人导航等领域。

LPA*

基本概念

LPA*（Lifelong Planning A*）算法是一种用于路径规划的增量式搜索算法，旨在解决动态环境中的路径规划问题。它是A*算法的改进版本，具有以下基本概念和特点：

增量式搜索：LPA*是一种增量式搜索算法，它可以逐步构建路径规划并在需要时进行更新。这使得它适用于动态环境，其中障碍物的位置或代价可能随时间变化。
代价地图：LPA*维护一个代价地图，其中每个节点都有一个代价值（cost）。这个代价值表示从起始节点到该节点的路径代价。初始时，只有起始节点的代价值为零，其他节点的代价值未知。
双向搜索：LPA*同时从起始节点和目标节点执行搜索，以便在搜索过程中快速找到一条路径。这有助于提高路径规划的效率。
代价更新：当障碍物移动或代价发生变化时，LPA*可以快速更新路径，以考虑新的信息。它只需要更新受影响的节点，而不需要重新计算整个路径。LPA*算法使用启发式函数来估计从当前节点到目标节点的代价。启发式函数可以是欧式距离、曼哈顿距离等。
最优性保证：与A*类似，LPA*可以保证找到最短路径（最低代价路径），前提是代价函数满足一致性条件（Admissible and Consistent）。
扩展节点：LPA*通过扩展具有最小代价的节点来构建路径。这些节点通常是距离起始节点最近的未扩展节点。
替代路径：LPA*不仅寻找最佳路径，还保留备选路径信息。这有助于在需要时切换到替代路径，以适应环境变化或障碍物的出现。
总之，LPA*算法是一种适用于动态环境下的路径规划算法，具有增量搜索、代价地图和最优性保证等特点。它可以用于自主机器人导航、自动驾驶车辆、游戏开发和其他需要实时路径规划的应用中。

基本流程

初始化：设置起始节点的代价为0，其他节点的代价为无穷大。将起始节点加入优先级队列，并设置其优先级为起始节点的代价值加上启发式函数的值。
循环直到找到路径或无法到达目标节点： a. 从优先级队列中取出优先级最高的节点。 b. 检查该节点是否已经更新过。如果已经更新过，则跳过该节点，继续下一轮循环。 c. 更新该节点的代价值为其父节点的代价值加上到达父节点的边的代价。 d. 如果该节点是目标节点，则找到了一条路径。结束算法。 e. 更新与该节点相邻的节点的代价值和优先级。如果更新了节点的代价值，将其加入优先级队列。
结束：如果优先级队列为空而无法找到路径，则表示无法到达目标节点。
LPA*算法通过不断地选择和更新优先级队列中的节点，逐步优化代价值，直到找到一条到达目标节点的路径或者确定无法到达目标节点。在每次更新节点时，需要考虑节点的代价值和启发式函数的值来确定优先级。

在A*的基础上，延伸到了动态环境，与D*lite算法蛮类似的，都是动态环境，都加入了启发函数。

D*lite

基本概念

D*Lite算法是一种路径规划算法，旨在解决在动态环境中的最短路径问题。它是对D算法的改进和优化。

D*Lite算法的基本概念如下：

初始化：首先，需要知道起始点和目标点。算法会根据当前的环境状态创建一个图，并将起始点设置为当前位置。
估计代价：使用启发式函数来估计当前位置到目标点的代价。这个代价可以是直线距离、曼哈顿距离等。
生成路径：根据启发式函数，选择具有最小估计代价的邻居节点作为下一个要访问的节点。然后，更新当前位置为选择的邻居节点，并将其添加到路径中。
动态更新：如果有新的信息（例如，环境中的障碍物发生变化），D*Lite算法可以根据这些信息动态更新路径。它会重新评估受到影响的节点，并相应地调整路径。
迭代优化：重复执行生成路径和动态更新的步骤，直到找到从起始点到目标点的最短路径。

DLite算法的优点在于它能够在动态环境中实时更新路径，并能够处理障碍物的变化。这使得它在机器人导航、无人机路径规划等领域有着广泛的应用。如果你想深入了解DLite算法的实现细节和应用示例，我建议你查阅相关的学术论文和在线资源。

基本流程

初始化起始点和目标点。
估计当前位置到目标点的代价。
选择邻居节点中具有最小估计代价的节点，并更新当前位置和路径。
如果有新的信息，动态更新节点和路径。
重复步骤3和步骤4直到找到最短路径。

在D算法的基础上加了个和A*算法类似的启发函数，对当前情况进行评估，从而选出最优。

各路径规划算法之间的区别

Dijkstra算法对于带权图中任意两个节点之间的最短路径问题非常有效，但在大规模图和复杂环境中，由于需要搜索所有可达节点，其时间和空间复杂度较高。
为了解决Dijkstra算法效率低的问题，A*算法作为一种启发式算法被提出。该算法在广度优先的基础上加入了一个估价函数。通过合理设计启发函数（估价函数），可以将搜索范围缩小到可能最佳路径周围的区域，从而大大减少搜索节点的数量。确保按照最小的总代价进行搜索。

简单来说，Dijkstra算法不错，但是节点多的时候，挨着搜还是太慢了，不如加个估价函数，然后先评估一下哪个地方可能最佳，直接缩小节点数量，提高效率，由此提出了A*算法

当然，我们点与点之间的关系，不可能只有距离，有时还用一种权重去表示，这样，就有了负权边，负权边可以在某些特定情况下引入更复杂的问题，因为它们与正权边不同，可能会影响最短路径计算的结果。

对于一些最短路径算法，如Dijkstra算法，其前提条件是边的权重必须为非负数。而Floyd算法则可以处理带有负权边的图。
A*算法适用于在静态环境中搜索单源最短路径，但是环境是不停的变化的，比如说玩游戏的自动寻路，他不可能一直直着走，也会拐弯，需要避障，这是就提出了在动态环境下进行路径规划的D*算法。D*算法是增量式的，D*算法不是从起点开始，而是从终点开始，不断向起点方向搜索路径，并根据环境的变化动态更新路径，但是，D*算法需要提前获取一条路径进行参考，并且在动态环境中需要实时响应变化，可能会受到计算和存储开销的限制。D*算法适用于在动态环境中进行路径重新规划。
D*算法为了避障，每次更新都会遍历整个路径，可能导致较高的计算复杂度。对于大规模环境或频繁更新路径的情况，性能可能会有所下降。所以DLite算法在D算法的基础上引入了启发式函数和优先队列，以减少不必要的计算量和提高搜索效率。通过使用与A类似的启发式函数，D*Lite算法能够更好地选择节点进行扩展，从而优化路径规划的质量和性能。
LPA*算法是基于A*算法的扩展，用于解决动态环境下的路径规划问题。它通过维护节点的当前代价值和参考代价值，以及一个更新列表，通过多次迭代搜索和增量更新策略来实现路径的快速更新。LPA*算法和D*算法都是增量式路径规划算法，用于处理动态环境下的路径更新问题。相比于D*算法，LPA*算法引入了参考代价值和更新列表，采用多次迭代搜索和增量更新策略，更加高效地进行路径更新。具体选择哪种算法应根据具体问题需求和环境特点进行评估。
RRT算法适用于在复杂、高维空间中的路径规划。
Floyd算法适合找寻任意两点之间的最短路径。