自动驾驶：路径规划概述

全局路径规划
局部路径规划
- DWA（动态窗口法）算法
- TEB（时间弹性带）算法
- [Lattice Planner算法](#Lattice Planner算法)
- [EM Planner算法](#EM Planner算法)

本博客从一些常见的全局路径规划算法和局部路径规划算法出发，介绍了它们的工作原理和优缺点。

全局路径规划

以全局地图，起始点和终点为输入，全局轨迹为输出，规划出一条无碰撞、路径最短且较为平滑的路径。

Dijkstra算法

Dijkstra算法是一种用于图中最短路径搜索的经典算法，由荷兰计算机科学家Edsger W. Dijkstra于1956年提出。它被广泛应用于网络路由、地图导航、运输规划等领域。Dijkstra算法通过找到从一个起始节点到所有其他节点的最短路径来解决问题。

原理

初始化：选择一个起始节点，将起始节点的距离设置为0，将所有其他节点的距离设置为无穷大。
选取最近节点：从未处理的节点中选择距离起始节点最近的节点。初始时，这个节点就是起始节点。
更新距离：对于选中的节点，计算从起始节点经过它到达其邻居节点的距离。如果通过选中节点到达邻居节点的距离小于邻居节点当前的距离，更新邻居节点的距离为新计算的距离。
标记为已处理：将选中的节点标记为已处理，表示已找到从起始节点到达它的最短路径。
重复步骤2至4：重复选取最近节点、更新距离和标记为已处理的过程，直到所有节点都被处理或者目标节点被处理。
路径回溯：一旦目标节点被处理，可以从目标节点回溯到起始节点，构建最短路径。

优点

确保最优解： Dijkstra算法保证找到的路径是从起始节点到目标节点的最短路径，前提是所有边的权重必须为非负数。
适用于单源最短路径问题： Dijkstra算法通常用于解决单源最短路径问题，即从一个起始节点到所有其他节点的最短路径。
易于理解和实现：算法逻辑相对简单，易于理解和实现。

缺点

无法处理负权重边： Dijkstra算法不能处理带有负权重边的图，因为它的工作原理基于不断减小节点的距离，负权重边可能导致无限循环。
不适用于大规模图：在大规模图中，Dijkstra算法的计算复杂度较高，可能不适用于实时应用。

A*算法

A算法（A-star算法）是一种启发式搜索算法，用于在图或图类问题中找到从起点到目标点的最短路径。这个算法结合了Dijkstra算法的最短路径性质和启发式搜索的优势，被广泛应用于路径规划、游戏AI、机器人导航和地图路线规划等领域。

原理

初始化：选择一个起始节点，将起始节点的代价设置为0，并将其放入一个待探索节点列表中。同时，为每个节点记录一个估计到目标节点的代价，通常使用启发式函数（heuristic function）来估计。
选择节点：从待探索节点列表中选择一个节点，通常是具有最小总代价（实际代价加上估计代价）的节点。初始时，起始节点是唯一的候选。
扩展节点：对于选中的节点，遍历它的邻居节点。计算从起始节点经过当前节点到达邻居节点的实际代价，并加上从邻居节点到目标节点的估计代价。如果这个总代价比邻居节点当前的总代价小，更新邻居节点的总代价，并将当前节点设置为邻居节点的父节点。
标记节点：将选中的节点标记为已探索，从待探索节点列表中移除。
重复步骤2至4：重复选择、扩展和标记节点的过程，直到找到目标节点或者待探索节点列表为空。
路径回溯：一旦找到目标节点，可以从目标节点回溯到起始节点，构建最短路径。

优点

（有条件）确保最优解： A*算法保证找到的路径是从起始节点到目标节点的最短路径，前提是启发式函数满足一定条件（被称为"一致性"或"单调性"）。
高效性： A*算法通常比纯粹的Dijkstra算法更快，因为它有能力通过启发式函数引导搜索，减少不必要的节点扩展。
适用性广泛： A*算法适用于各种问题，包括路径规划、游戏AI、机器人导航和地图路线规划等。

缺点

启发式函数选择： A*算法的性能高度依赖于所选择的启发式函数。选择不合适的启发式函数可能导致算法不准确或不高效。
存储需求： A*算法需要存储和管理待探索节点列表，对于大规模问题可能需要较多的内存。

RRT（随机快速探索树）算法

RRT（Randomized Rapidly-Exploring Tree）是一种用于路径规划的随机采样算法，最初由Steven M. LaValle于1998年提出。RRT旨在解决机器人或其他自主体的运动规划问题，使它们能够在未知或复杂的环境中找到有效路径。

原理

初始化：从起始点开始，创建一棵树，该树仅包含起始点。
随机采样：在地图空间中随机生成一个点，这个点通常是随机的，但也可以考虑环境的先验知识。
连接到树：找到离采样点最近的树上的节点，然后从这个节点向采样点生成一条边。这个边的生成通常遵循一些规则，例如最大距离限制，以确保树的扩展不会太快。
重复：重复步骤2和3，直到生成树的一部分接近目标区域。在每一步，RRT都在树上添加一个新节点和一条新边，以逐步扩展搜索空间。
连接到目标：一旦树的一部分靠近目标点，就可以在树上找到一条路径，从起始点连接到目标点。
路径优化（可选）：可以对生成的路径进行优化，以提高路径的平滑性和效率。

优点

适应高维度环境： RRT适用于高维度状态空间，因此对于具有大量自由度的机器人或问题来说非常有用。
多样化的路径：由于随机性质，RRT能够生成多样化的路径，有助于克服局部最小值问题。
实时规划： RRT的增量性质使其适用于实时路径规划，可以在机器人运动时重新规划路径。

缺点

不能确保最优路径： RRT不一定总是找到全局最优路径，它更关注于搜索过程的快速探索和多样性。
收敛速度不确定： RRT的收敛速度依赖于随机采样和树的生长规则，因此在某些情况下可能需要较长时间才能找到合适的路径。
有限的环境模型： RRT假设了机器人可以自由移动，不考虑动力学约束，因此在某些情况下可能不适用。

PRM（概率路线图）算法

PRM（Probabilistic Roadmap）是一种用于路径规划的概率算法，特别适用于高维度和复杂的环境。PRM算法通过在地图上随机生成一组采样点，然后连接这些点来构建一个图，从而生成路径。PRM算法在机器人导航、运动规划和自主机器人领域得到广泛应用。以下是PRM算法的基本工作原理：

原理

采样：随机在地图上生成一组采样点，这些点通常是机器人可以合法到达的位置。采样点的数量和分布可以根据问题的复杂性和需求进行调整。
连接采样点：对于每个采样点，检查其附近是否有其他采样点，如果有，则将它们连接起来，形成边。连接两个采样点的边通常表示机器人可以在这两个点之间移动的路径。
检查可通行性：对于连接的边，进行可通行性检查，以确保路径不会与障碍物相交。如果某条边不满足可通行性要求，将其删除。
路径搜索：使用标准路径搜索算法（如Dijkstra或A*）在生成的图上查找最短路径。起始点和目标点通常与最近的采样点连接，从而确保路径在图中。
路径优化（可选）：可以对生成的路径进行优化，以提高路径的平滑性和效率。

优点

适应高维度环境： PRM算法适用于高维度状态空间，对于具有大量自由度的机器人或问题非常有用。
支持多个运动约束： PRM算法可以轻松地集成多个运动约束，例如最小转弯半径、最大速度、最大加速度等，以满足不同机器人的运动要求。
全局路径规划： PRM算法可以用于全局路径规划，确保找到连接起始点和目标点的路径。
预先规划多个路径：通过生成一组采样点，PRM算法可以预先规划多个路径，以备不时之需。

缺点

构建路线图需要时间：构建PRM的路线图可能需要大量计算时间，特别是在高维度环境中。
路径搜索复杂度：路径搜索阶段的计算复杂度取决于图的大小和形状，可能在某些情况下较高。
不一定最优： PRM算法不一定总是找到全局最优路径，它的性能取决于采样点的分布和数量。

局部路径规划

以局部地图，周围环境信息作为输入，局部轨迹为输出，规划出一条无碰撞满足运动学或动力学约束的轨迹，常用于避免碰撞、满足舒适性等要求和实时根据周围环境进行路径的调整。

DWA（动态窗口法）算法

DWA（Dynamic Window Approach）算法是一种用于移动机器人路径规划和避障的实时控制方法。它通常用于机器人在动态环境中避免碰撞和实时规划路径。DWA算法是一种模型预测控制方法，它通过在机器人的状态空间中生成一系列可能的运动轨迹，然后评估每个轨迹的性能来选择最佳行动。

原理

状态空间建模：将机器人的状态空间表示为位姿（位置和方向）和速度（线速度和角速度）的组合。这个状态空间描述了机器人在环境中的位置和运动状态。
生成运动轨迹：在状态空间中生成一系列可能的运动轨迹，通常通过在速度空间（线速度和角速度）中采样来实现。这些轨迹代表了机器人可能的运动选择。
评估轨迹：对于每个生成的轨迹，评估它们的性能。性能评估通常包括两个方面：运动轨迹能否到达目标位置（目标导向性），以及轨迹是否避开了障碍物（避障性）。这些评估指标通常基于预测模型和环境感知。
选择最佳行动：选择性能最佳的轨迹作为机器人的下一步行动。通常，最佳轨迹被定义为在目标导向性和避障性之间取得平衡的轨迹。
控制机器人：执行选择的行动，控制机器人进行移动。机器人执行当前轨迹上的运动命令，然后重新进行路径规划以进行下一步移动。
实时更新：重复上述步骤以实现实时路径规划和避障，不断更新机器人的运动。

优点

实时性： DWA算法适用于实时运动规划，能够在机器人移动时快速生成适应环境变化的轨迹。
适应性：由于DWA算法的实时性，它能够适应动态环境中的障碍物移动和变化。
避障能力： DWA算法通过避免评估避障性能来避免碰撞，使机器人能够在复杂环境中导航。

缺点

局部搜索： DWA算法通常是局部搜索方法，因此不一定总是找到全局最优路径。
性能依赖于参数： DWA算法的性能高度依赖于参数的选择，特别是与性能评估相关的参数。这需要进行调试和优化。

TEB（时间弹性带）算法

TEB（Time-Elastic Band）算法是一种用于机器人运动规划和轨迹优化的方法，特别适用于移动机器人在动态环境中的路径规划。TEB算法旨在解决机器人在复杂和动态环境中的路径规划问题，以确保机器人能够安全、高效地移动。

原理

初始化：

确定机器人的初始状态，包括位置和速度。

设置时间分辨率（时间步长）和速度分辨率（速度步长）等参数。

建立一个时间-空间轨迹，包含初始状态。
目标设定：

确定机器人的目标位置。
路径生成：

在时间-空间轨迹中，逐个时间步骤生成可能的位置-速度对（位姿和速度）。

考虑机器人的动力学模型，以确保生成的轨迹在物理上合理。

考虑速度分辨率，生成多个不同速度的轨迹分支。
避障检测：

对于每个生成的轨迹，检查轨迹上的每个时间步骤是否与障碍物相交。

如果某个时间步骤与障碍物相交，将该轨迹标记为不合法。
性能评估：

对于合法的轨迹，评估它们的性能。

性能评估通常包括目标导向性（轨迹是否接近目标位置）、避障性（轨迹是否避开障碍物）以及平滑性等。
轨迹选择：

根据性能评估选择最佳的轨迹或轨迹分支作为机器人的下一步行动。

最佳轨迹通常是在目标导向性和避障性之间取得平衡的轨迹。
控制机器人：

执行选择的轨迹上的运动命令，将机器人移动到下一个状态。
实时更新：

在机器人移动过程中，不断更新轨迹和重新规划，以适应环境的动态变化。
达到目标：

当机器人接近目标位置并满足终止条件时，算法结束。

优点

动态适应性： TEB算法具有动态调整轨迹的能力，可以在实时感知到的环境信息下进行路径的重新规划。这使得它适用于处理动态障碍物和环境变化的情况。
轨迹平滑性： TEB算法倾向于生成平滑的轨迹，有助于提高机器人的运动舒适性和稳定性。
速度控制： TEB算法允许对机器人的速度进行动态调整，以适应环境中的障碍物密度和机器人的运动能力。
支持多个运动约束： TEB算法可以轻松地集成多个运动约束，例如最小转弯半径、最大速度、最大加速度等，以满足不同机器人的运动要求。

缺点

计算复杂度： TEB算法的计算复杂度相对较高，特别是在高维度状态空间中，可能导致实时性方面的挑战。
对参数敏感： TEB算法依赖于许多参数，如时间分辨率、空间分辨率等。选择适当的参数对算法的性能至关重要，但可能需要一些调试。

Lattice Planner算法

Lattice Planner是一种基于采样的路径规划算法，适用于高维度状态空间和具有复杂运动约束的机器人。它将机器人的状态空间建模为一个高维度格点图，通过采样生成候选轨迹、利用各项指标评估来寻找最佳路径。

原理

状态空间建模：将机器人的状态空间表示为高维度格点图。
生成候选轨迹：在状态空间中生成一组候选轨迹，每个轨迹代表机器人可能的运动路径。
碰撞检测：对每个候选轨迹进行碰撞检测，确保轨迹不与障碍物相交。
性能评估：对合法的轨迹进行性能评估，考虑目标导向性、避障性、平滑性和舒适性等指标。
选择最优轨迹：确定从起始状态到目标状态的最优路径。
轨迹优化：可选地对生成的路径进行优化。

优点

适应高维度状态空间和复杂运动约束。
支持多个运动约束。

缺点

计算复杂度较高。
参数选择对性能影响较大。

EM Planner算法

EM Planner是一种采用期望最大化（Expectation-Maximization, EM）方法的路径规划算法，用于解决机器人在未知环境中的路径规划问题。它通过估计未知环境的概率分布来规划路径。

环境建模：使用传感器数据构建环境的概率地图，其中包括障碍物的位置和不确定性。

EM算法：使用EM算法来估计未知环境的概率分布，包括障碍物的分布和不确定性。

路径规划：基于估计的环境概率分布，使用路径规划算法来规划路径。

轨迹执行：执行规划的路径，同时不断更新环境模型和路径以适应新的传感器数据。

优点

适用于未知环境和不确定的环境，能够估计环境的概率分布。

缺点

较高的计算复杂度。