最新综述！详解基于采样的运动规划的过去、现在和未来！

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

今天自动驾驶之心很荣幸邀请到Garfield分享运动规划的最新综述，如果您有相关工作需要分享，请在文末联系我们！

>>点击进入→自动驾驶之心【规划控制】技术交流群  

论文作者 | Garfield

编辑 | 自动驾驶之心

Sampling-Based Motion Planning: A Comparative Review

作者：Garfield

1. 引言

首先，survey提到在过去几十年中，机器人技术取得了显著进展，其中采样式运动规划方法的发展是一个增长的领域。这些方法利用采样机器人的配置（即机器人的位置和姿态）来快速找到可行的、在某些情况下甚至是最优的机器人运动路径。这种方法已经被应用于多个领域，如机器人建筑、多机器人协调、自动驾驶、工业制造和蛋白质折叠等。

然而，尽管取得了成功，但当前的研究人员和从业者却缺乏使用SBMP方法的最新准则。特别是，目前还没有一部历史性的论著总结了该领域的发展情况，包括迄今为止的进展。同时，还没有对规划器进行分类，总结了过去十年的发展。此外，人们往往不清楚SBMP方法与其他运动生成框架（如运动优化、运动基元、基于搜索的规划或基于控制的规划）的区别。最后，人们往往不清楚哪种规划器最适合特定的应用领域，以及在特定情景中哪种规划器的性能最佳。

因此，当前的研究工作需要解决的问题包括：

1. 提供一份最新的使用SBMP方法的指南；

2. 撰写一部历史性的著作，回顾该领域的发展情况；

3. 对规划器进行分类，总结过去十年的发展；

4. 解释SBMP方法与其他运动生成框架的区别；

5. 确定特定应用领域最适合的规划器类型，并找出特定情景下性能最佳的规划器。

这些问题的解决将有助于推动SBMP方法的进一步发展和应用。

2. Motion Planning History and the Emergence of Sampling-based Methods

Motion Planning History and the Emergence of Sampling-based Methods可以分为四个阶段：

1. 预采样时代（1979-1989）：

• Lozano-Pérez引入了配置空间的概念。

• 对运动规划问题进行分析和计算复杂度研究。

• 引入人工势场用于机器人控制。

采样兴起时代（1990-1999）：

• 采样方法的先驱工作。

• 引入基于图的规划器（如PRM：概率路图规划器）和基于树的规划器（如RRT：快速随机树）。

采样巩固时代（2000-2009）：

• 关注改进采样方法。

• 开发偏向采样技术以实现高效的探索。

• 探索能力估计和动态域RRT。

最优性和学习时代（2010至今）：

• 开发渐近最优规划器：PRM和RRT。

• 将机器学习技术整合到运动规划中。

• 利用过去的规划经验加快搜索速度。

• 学习采样分布和利用深度学习进行运动规划。

总的来说，运动规划的历史经历了从预采样方法到采样方法的兴起和巩固的过程。采样方法在解决运动规划问题方面显示出高效性，并在最优性和机器学习技术整合方面得到进一步改进。

3. Methods

运动规划方法可以分为经典方法和现代方法。经典方法主要基于图搜索和采样策略，而现代方法则利用优化技术来解决运动规划问题。下面将介绍几种常见的运动规划方法：

1. 图搜索方法：图搜索方法将运动规划问题建模为图的搜索问题。其中，每个图节点表示机器人的一个状态，图的边表示状态之间的转换。常见的图搜索算法包括A算法和Dijkstra算法。A算法通过启发式函数来引导搜索过程，以找到最优路径。Dijkstra算法则通过逐步扩展距离起始节点最近的节点来搜索路径。

2. 采样-based方法：采样-based方法通过在状态空间中进行采样来搜索路径。其中，随机采样轨迹（RRT）和概率路线图（PRM）是常用的算法。RRT算法从起始状态开始，通过随机采样和连接操作逐步扩展树结构，直到找到目标状态。PRM算法首先随机采样一组状态，并通过连接可行状态来构建一个图。然后，使用图搜索算法在图上搜索路径。

3. 优化-based方法：优化-based方法使用优化技术来找到最优路径。其中，Covariant Hamiltonian Optimization for Motion Planning (CHOMP)算法和Constrained Forward-Backward Search (CFS)算法是常见的方法。CHOMP算法通过迭代优化路径，以最小化代价函数。CFS算法将运动规划问题转化为一个约束优化问题，并使用前向和后向搜索来找到最优路径。

4. 基于采样的运动规划方法：该方法使用采样函数和局部规划器来隐式表示状态空间，并生成与状态空间密集对应的无限状态序列。采样函数可以是无偏或有偏的，其中有偏方法可以根据不同的偏差方式来改变采样的概率分布。基于障碍物的采样是一种常见的偏差方法，它倾向于选择靠近障碍物的状态。这种偏差可以改善狭窄通道中的规划结果，但也可能引入路径长度方面的偏差。基于间隙的采样是另一种方法，它优先选择具有更大间隙（机器人与环境之间的距离）的样本。这种方法可以缓解执行不确定性，但计算间隙查询通常较为昂贵。确定性采样是第三种方法，它生成确定性的采样序列，可以偏向于最优路径或具有低离散度的序列。这些基于采样的方法可以应用于各种运动规划问题，并且已经提出了许多改进方法来增强其性能和适用性。这些方法可以根据规划器的类型进行分类，并可以针对特定的问题进行扩展和优化。

除了这些方法，还有一些其他的方法用于特定的运动规划问题。例如，对于机器人在复杂环境中的移动，可能会使用基于感知的运动规划方法，通过感知传感器获取环境信息，并根据环境信息进行路径规划。另外，对于具有动力学约束的机器人系统，可以使用运动动力学规划方法，考虑机器人的动力学特性来生成合理的轨迹。需要根据具体的应用场景和要求选择适当的运动规划方法。不同的方法具有不同的优势和适用性，可以根据问题的复杂性、环境约束和机器人特性等因素进行选择。

4. 拓展部分

然后survey介绍了基于采样的运动规划方法的扩展问题。这些扩展问题在状态空间上引入了额外的结构，可以通过采样法规划器进行扩展。

其中一种扩展是基于投影的运动规划。通过引入投影，可以简化许多高维问题，并允许在不同的抽象级别上进行规划。投影可以作为有偏采样器或用于调整和引导采样的方式进行规划。如果投影是可接受的，即解决简化问题是原始问题的必要条件，那么可以保证渐近最优性等性质。尽管投影通常显著加快了规划的速度，但对于新的问题领域来说，定义投影往往是困难的。

另一种扩展是可微分运动规划。大多数在运动规划中使用的函数，如成本、目标或约束，通常是可微分的。利用这些可微分函数是可微分运动规划的研究方向。虽然大多数基于采样的规划器避免使用可微分信息，但有证据表明，利用可微分信息可以更快地收敛到最优解。可微分规划器需要仔细权衡何时以及是否计算可微分信息。最近的研究将优化与采样相结合，通过添加图优化、改进近似有效路径或优化转向函数等方式来实现。

在规划中考虑流形约束是另一种扩展。在经典运动规划中，大多数约束可以通过采样来构造可满足的状态空间元素。然而，复杂的约束（如机器人与环境之间的接触）通常会引入在状态空间中具有零测度的区域，这些区域几乎不可能被采样到。处理这些约束的方法被称为运动规划中的流形约束。处理这些约束的方法有很多，例如可以放宽约束的体积以简化问题，或将随机样本投影回约束区域。

运动规划中的动态环境是另一个常见扩展。在实际应用中，机器人常常需要处理移动、出现或消失的障碍物。处理这些障碍物的方法被研究为动态环境中的规划。由于成功的规划可能会失效，因此需要专门的规划器。例如，RRTX可以在运行时更新以适应目标为中心的搜索树，预计算路网可以快速重新计算解决方案路径等。

在许多实际情况下，机器人无法完全观测环境，而需要使用传感器来探测世界。这就引出了部分可观测环境中的规划问题。为了解决这些情况，可以对置信空间进行采样，其中包含有关世界和机器人状态的假设。由于假设空间可能很大，因此重要的是正确简化这些空间以进行搜索。

最后，还有一种扩展是处理外部力场的规划。机器人经常需要处理来自重力、摩擦或风等外部力的影响。基于采样的运动规划框架可以将这些力集成为机器人状态空间上的向量场。在该领域的规划方法主要关注处理无人机中的风扰动或自主车辆中的侧风等问题。

这些扩展问题使得基于采样的运动规划方法可以适应各种实际应用中的挑战。通过引入额外的结构和技术，可以处理高维问题、可微分函数、流形约束、动态环境、部分可观测环境和外部力场等情况。这些扩展问题的研究旨在提高运动规划的效率、可靠性和适应性，以便机器人能够在复杂和不确定的环境中执行各种任务。

5. 有竞争性的运动生成框架

竞争性运动生成框架包括以下几个部分：

1. 运动优化：

• 运动优化将运动生成问题形式化为优化问题。

• 无梯度方法（例如，简化路径、混合方法）专注于改善路径长度和平滑性。

• 有梯度方法（例如，CHOMP、TrajOpt、KOMO、GPMP）通过对成本、目标和约束进行微分快速找到低成本路径。

• 优点：快速收敛到低成本解，能够处理碰撞。

• 缺点：局部最优解，缺乏完整性或最优性保证，可能产生无效路径。

1. 运动基元：

• 运动基元是预定义或学习得到的用于完成特定任务的运动。

• 基元可以在状态空间中定义为动力学系统，使机器人能够朝向目标或避开障碍物。

• 优点：反应式规划，实时障碍物避让，适用于需要反馈的任务。

• 缺点：缺乏完整性或最优性保证，寻找和微调基元困难。

搜索规划：

• 搜索规划将网格 imposed 到状态空间中，并构建状态空间图以进行高效搜索。

• 基于此图，类似A*的算法可以在网格分辨率内找到最优路径。

• 优点：在网格分辨率内有优化保证，可使用开源软件（例如，SBPL）。

• 缺点：仅在网格分辨率内能够提供一定的完整性保证，细分辨率会导致规划时间昂贵，高维空间中的使用困难。

基于控制的规划：

• 控制算法将机器人驱动到期望的目标状态。

• 示例包括PID控制器、人工势场、LQR、MPC和强化学习。

• 控制器可以是反应式的、局部最优的或使用强化学习方法学习得到的。

• 优点：快速计算局部最优路径，反应性，可结合执行反馈。

• 缺点：可能陷入局部最小值，返回次优路径，没有完整性或最优性保证，计算时间限制。

每个框架都有其优势和局限性，选择取决于具体的运动生成问题的要求。

6. 评测

根据给定的评测结果，从OMPL库中选择了几个规划算法，并在一组24个场景上进行了评估。每个场景选择了一组适用的规划器，并以与场景相关的超时时间运行了100次。所有实验都使用了规划器的默认参数。更改这些参数可能会对结果产生影响，但实验数据集的规模限制了参数的微调。

评测使用了一台装有Ubuntu 16.04操作系统的4核8GB RAM的笔记本电脑作为硬件平台。评测结果以成功成本图的形式呈现，其中时间在x轴上，成功率和解决方案成本在y轴上。对于非优化规划器，显示了第一个解决方案的平均成本。如果没有显示颜色，则表示该规划器未能找到任何解决路径。

评测分为几个部分，以下是每个部分的概要：

1. Classical Experiments: 这一部分包括一些经典的运动规划问题，每个场景中的任务是将一个刚体从起始区域移动到目标区域。评估结果显示，BIT、EST、FMT和RRT-Connect在成功率方面表现良好，其中FMT在Home场景中的成功率达到了50%。在成本方面，BIT在5个场景中的收敛性最好，而基于图的规划器如PRM在解决这些问题上需要更长的时间。

2. Manipulation Experiments: 这一部分包括来自MotionBenchMaker数据集的问题，每个问题中对象的位置相对于机器人是变化的。评估结果显示，RRT-Connect的成功率最高，在5个场景中达到了100%。在成本方面，PRM*在4个场景中收敛到了低成本解决方案。

3. Narrow Passage Experiments: 这一部分包括六种不同类型的狭窄通道问题。评估结果显示，RRT-Connect和TRRT在成功率方面表现最好，几乎可以在3个场景中达到100%的成功率。在成本方面，只有FMT在5个场景中能够找到低成本解决方案。

4. Extension Experiments: 这一部分包括具有接触约束的问题和使用动态模型驱动机器人的场景。评估结果显示，使用基于约束的框架的规划器（如EST、BIT* 和RRT-Connect）在某些场景中可以达到100%的成功率。在动态模型的情况下，只有Kinodynamic-RRT能够找到解决方案，而最优规划器SST* 无法解决任何场景。

总体而言，评估结果表明，在不同的场景下，不同的规划算法具有各自的优势。选择合适的规划器取决于机器人和环境的特点。

7.结论

本研究通过对OMPL规划库中多个规划算法的评估，对不同类型的运动规划问题进行了广泛的实验。评估结果显示，在不同的场景和任务下，不同的规划算法表现出各自的优势。

在经典的运动规划问题中，BIT* 、EST、FMT和RRT-Connect展现出较高的成功率，并且BIT* 在成本收敛性方面表现最好，而PRM类型的规划器需要更长的时间来找到解决方案。

在操作问题中，RRT-Connect在成功率方面表现最好，而PRM*在成本收敛性方面表现较好。

在狭窄通道问题中，RRT-Connect和TRRT在成功率方面表现出色，而FMT在成本收敛性方面表现最好。

在具有接触约束和动态模型的问题中，基于约束的规划器（如EST、BIT* 和RRT-Connect）在某些场景中表现出较高的成功率。然而，在动态模型的情况下，只有Kinodynamic-RRT能够找到解决方案，而SST* 无法解决任何场景。

综合评估结果，我们可以得出结论，选择适合特定机器人和环境的规划算法至关重要。不同的算法在不同的场景和任务下表现出不同的优势和劣势。因此，为了获得高效的运动规划解决方案，需要仔细选择和调整规划算法，并考虑问题的特性和要求。

本研究对规划算法的评估提供了有价值的参考，但在实际应用中仍需要进一步的研究和实验。未来的工作可以进一步探索参数调整、算法组合和问题建模等方面，以提高规划算法的性能和适应性。

① 全网独家视频课程

BEV感知、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、点云3D目标检测、目标跟踪、Occupancy、cuda与TensorRT模型部署、协同感知、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码即可学习）

视频官网：www.zdjszx.com

② 国内首个自动驾驶学习社区

近2000人的交流社区，涉及30+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（2D检测、分割、2D/3D车道线、BEV感知、3D目标检测、Occupancy、多传感器融合、多传感器标定、目标跟踪、光流估计）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！

③【自动驾驶之心】技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多模态感知、Occupancy、多传感器融合、transformer、大模型、点云处理、端到端自动驾驶、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向。扫码添加汽车人助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）

④【自动驾驶之心】平台矩阵，欢迎联系我们！