基于物理的运动规划评估标准

基于物理的运动规划：评估标准和基准测试

1 引言

机器人操作需要精确的运动规划与控制来执行任务，无论是工业机器人、移动操作器还是人形机器人。必须确定一种安全地将机器人从起始状态导航到目标状态的方法，同时满足动力学的（几何的和微分的）约束，以及引入由可能接触和世界动态属性（如重力和摩擦）所施加的基于物理的约束[1, 2]。这些问题显著增加了计算复杂度，因为某些无碰撞的几何路径在存在这些约束的情况下可能不可行。

因此，基于物理的运动规划作为一种新型规划算法应运而生，它在考虑动力学约束的同时，还引入了基于物理的约束，即可视为对动力学运动规划[3]的扩展，进一步考虑刚体之间的动力学相互作用，实现对物体的有目的操控。这种相互作用基于基本牛顿力学原理进行仿真，仿真结果被用于规划过程。基于物理的规划器性能在很大程度上取决于所选用的动力学运动规划器，后者隐式地用于状态采样和解路径的构建。状态传播通过动力学引擎（如ODE[4],）来实现，该引擎融合了动力学的和基于物理的约束。

运动规划在其最简单的形式下（即作为几何问题）是PSPACE‐完全的[5]。引入动力学约束和基于物理的属性使其变得更加复杂且计算量更大，对于复杂系统，甚至基于物理的规划问题的可判定性也值得怀疑[6]。因此，为了使基于物理的规划在计算上可处理，使用最合适且计算高效的动力学规划器至关重要。在先前提出的基于物理的规划方法中，已采用了不同的动力学规划器和物理引擎。

一些研究对不同基于物理的规划框架内若干动力学运动规划器的性能进行了比较分析。例如，文献[7],提出的基于物理的规划算法采用了非确定性策略和技能来减小基于物理规划的搜索空间，并使用两种不同的动力学运动规划器——行为动力学快速探索随机树（BK‐RRT[8]）和平衡增长树（BGT[7]），在规划时间和树长度方面对其进行了评估。该研究采用物理引擎PhysX[9]作为状态传播器。另一种基于物理的规划方法[10]将基于采样的运动规划与离散搜索相结合，利用工作空间分解将规划问题映射为图搜索问题。该工作使用RRT、规划层协同组合（SyCLoP[11]）以及一种改进版SyCLoP作为动力学运动规划器，在规划时间方面评估了其性能。扩展步骤通过Bullet[12]物理引擎实现。第三种方法提出了一种基于物理的运动规划框架，该框架将操作知识以本体形式编码[13]。该方法通过对知识进行推理来提升计算效率，与简单的基于物理的规划相比，在规划时间和生成轨迹方面均表现出显著的性能提升。该研究使用了两种动力学运动规划器：基于内部‐外部单元探索的动力学规划（KPIECE[14]）和RRT。状态传播器采用开放动力学引擎（ODE）。

上述所有研究基本上都是测量不同动力学运动规划器的时间复杂度。由于基于物理的规划同时评估了动力学和基于物理的约束，仅基于规划时间的评估可能并不充分。因此需要新的评估标准，因为许多其他动力学参数（如消耗的功率、作用量、平滑度）可能会显著影响规划决策，例如在[15, 16]中提出的任务规划方法，该方法通过评估以消耗的功率和作用量为代价的成本，利用基于物理的推理过程来确定规划的可行性。鉴于此，本研究提出了一种新的用于基于物理的规划的基准测试标准，该标准结合了系统的动态特性（以确定解的质量）以及计算复杂度。该标准被用于在[13]提出的基于物理的规划框架内，对不同的动力学运动规划器（RRT、EST、KPIECE和SyCLoP）进行比较，该框架基于对本体操作知识的推理过程。

2 动力学的运动规划

运动规划问题涉及在构型空间（C）中计算从给定起始状态到目标状态的无碰撞轨迹，该空间是机器人所有可能构型的集合[17]。C的几何可达区域称为Cfree，障碍物区域被称为Cobs。诸如概率路线图[18]和快速探索随机树[19]等基于采样的算法在高维构型空间中的规划表现出显著性能。这些算法通过图或树将无碰撞构型连接起来，以捕捉Cfree的连通性，并沿这些数据结构寻找路径以连接初始构型和目标构型。

动力学运动规划指的是机器人的运动必须同时满足运动学约束（如关节极限和避障）以及某些动态约束（如施加的力、速度和加速度的界限[20]）的问题。基于树的规划器最适合考虑动力学约束[1],，因为其使用动力学方程来确定用于扩展树结构的产生的运动。基于采样的动力学运动规划器的基本功能是在记录系统动力学的高维状态空间S中进行搜索。机器人在位形q ∈ C下的状态定义为s=(q ˙q)。为了确定解，规划将在状态空间中进行，方式类似于C。本节简要回顾现有最常用的动力学运动规划器，这些规划器可分为三类：a) RRT和EST属于对状态进行采样的规划算法类别；b) KPIECE属于对运动或路径段进行采样的类别；c) SyCLoP是一种混合规划器，将规划问题分解为离散层和连续层。

快速探索随机树（RRT）

它是一种基于采样的动力学运动规划算法[21]，能够高效地探索高维构型空间。基于RRT的算法的工作机制是从起始状态（qstart ∈C）为根节点随机生长一棵树，直到在目标状态（qgoal ∈C）找到一个样本。树的生长基于两个步骤：选择和扩展。在第一步中，随机选择一个样本（qrand），然后在树中搜索其最近的节点（qnear）。第二步是从qnear施加一段随机控制（满足约束条件）作用一段时间。在到达的构型中，选择离qnear最近的一个作为qnew，并在树中添加一条从qnear到qnew的边。通过此过程，树上的所有路径都是可行的，即根据构造方式满足所有的动力学约束。

扩展空间树规划器（EST）

该方法在状态×时间空间[22][23]中构建一个树状路线图T。其思想是选择一个T中的里程碑，并从该点开始随机采样控制量并作用一段时间。如果最终状态位于自由空间，则将其作为新的里程碑加入到T中。用于扩展的里程碑的选择方式需确保生成的树既不会过于密集也不会过于稀疏。该动力学的规划器分为三个步骤：里程碑选择、控制选择和终局连接。在第一步中，以与里程碑m邻近的其他里程碑数量成反比的概率，从T中选择一个里程碑m。在第二步中，从选定的里程碑m出发，随机采样并施加控制量。由于在动力学约束下运动时可能无法精确到达目标状态，因此第三步终局连接会在目标周围定义一个区域，任何位于该区域内的里程碑都被视为已到达目标状态。

基于内外单元探索的动力学运动规划（KPIECE）

该规划器专为复杂动力系统设计。KPIECE通过从树节点中选择一个节点，并应用随机采样的控制量和随机采样的持续时间来扩展运动树。状态空间被投影到一个低维空间，该空间被划分为单元，以估计覆盖率。由于此投影，每条运动路径将属于某个单元，每个单元根据其相邻单元是否被占据而被分类为内部单元或外部单元。然后，基于重要性参数进行单元选择，该参数的计算依据如下：1）覆盖率（优先选择覆盖率较低的单元）；2）选择次数（被选择次数较少的单元优先）；3）邻居（邻居较少的单元优先）；4）选择时间（最近被选择过的单元优先）；5）扩展性（相较于扩展较慢的单元，优先选择更容易扩展的单元）。具有最高重要性的单元将被选中。此过程将持续进行，直到运动树到达目标区域。

规划层次的协同组合（SyCLoP）

这是一种元方法，将运动规划视为混合空间中的搜索问题（连续层和离散层）以高效求解运动学与动力学约束下的问题。连续层由状态空间表示（通过基于采样的运动规划器如RRT或EST进行探索），而离散层则由工作空间的分解确定。该分解用于计算一种称为引导值的成本参数，以引导运动规划器朝向目标区域。SyCLoP基于以下步骤运行：引导值计算和区域选择。引导值根据覆盖率和选择频率进行计算。前者由基于采样的运动规划器（连续层）获得，后者通过统计离散空间中每个单元被选择的次数来确定。区域的选择将基于该区域的可用自由体积进行（优先选择自由体积较大的区域进行探索）。此过程将持续进行，直到规划器在目标区域中找到一个样本为止。根据连续层所使用的规划器，SyCLoP将被称为SyCLoP‐RRT或SyCLoP‐EST。

2.1 本体论物理驱动的运动规划

基于物理的运动规划由一类新的规划算法组成，这些算法通过进一步考虑物体之间的相互作用以及可能的基于物理的约束（例如重力和摩擦，这些因素会影响操作动作及其结果），在物理真实性方面迈出了重要一步。寻找无碰撞轨迹不再是最终目标；现在可以允许与某些物体发生碰撞，即这些算法还会考虑操作动作（如推动动作）以计算合适的轨迹。刚体之间动态交互（用于操作）及其他基于物理的约束的引入，增加了状态空间的维度和计算复杂度。在某些情况下，特别是对于具有复杂动力学的系统，该问题甚至可能无法处理。

本体论基于物理的运动规划是一种最近提出的方法，旨在应对上述挑战[13]。该方法利用基于Prolog的推理过程来处理物体知识和操作动作（这些知识以本体的形式表示）。该推理过程用于提高计算效率，并使操作问题在计算上可处理。它采用一种混合方法，包含两个主要层次：基于知识的推理层和运动规划层。

基于知识的推理层利用操作本体推导出一种称为抽象知识的知识，该知识包含物体及其属性的信息（例如可操作区域，即可以从哪些区域推动物体），以及机器人的初始状态和目标状态。此外，抽象知识将物体分类为固定物体和可操作物体，其中可操作物体进一步分为自由可操作和约束导向可操作物体（例如，某些物体可以从任意区域被推动，而其他物体则只能从特定区域被推动）。可能只能从某个给定区域并沿某些预定义方向进行推动。此外，抽象知识还决定了物体的几何位置，以区分目标状态是否被占据。

运动规划层包含一个从抽象知识进行推导的推理过程。所推导出的知识称为实例化知识，是一种在每个时间实例处更新的动态知识。运动规划层采用基于采样的动力学运动规划器（如KPIECE或RRT）以及用作状态传播器的物理引擎。在扩展步骤之后，若所有物体均满足实例化知识所施加的操作约束（例如类车物体只能被向前或向后推动，因此任何导致其侧边发生碰撞的状态都将被禁止），则规划器接受该新状态。通过这种方式，由于无用的作用量被剪枝，规划器的树状数据结构的增长更加高效。

3 基准测试参数

现有的动力学运动规划器种类繁多，这些算法的规划策略在概念上各不相同，这会显著影响基于物理的规划的性能。本文提出了一种用于评估动力学运动规划器在基于物理的规划中性能的准则。建议除了计算复杂度之外，还应评估决定计算出的解路径质量的动力学参数。该评估通过估算机器人沿计算出的路径移动时消耗的功率、计算出的轨迹的总作用量（即本节后文将解释的轨迹的动力学属性）以及轨迹平滑度来确定。计算复杂度是基于每个规划器的规划时间和平均成功率计算得出的。若某规划器在上述准则下表现最优，则称其为最合适的规划器。

物理引擎（例如ODE、Bullet和PhysX）的选择可能不会影响仿真结果，因为它们的设计理念均基于基础物理原理，尽管性能和精度可能略有差异，但所有物理引擎的仿真结果几乎相同。

已建立以下性能参数，用于在本体论基于物理的运动规划器框架内评估不同的动力学运动规划器。动力学运动规划器给出的轨迹由一系列需依次施加的力及其持续时间描述，以使机器人移动（以无碰撞方式或可能推动某些可操作物体的方式）：

– 作用量 ：它是物理系统的一个动力学属性，定义为一个泛函 A，该泛函以定义轨迹的移动序列为输入，并返回一个标量数作为输出：

$$
A = \sum_{i} |f_i| \Delta t_i \varepsilon_i,
$$

其中 $ f_i $, $ \Delta t_i $ 和 $ \varepsilon_i $ 分别是施加的控制力、作用时间以及由此产生的位移矢量 $ d_i $。

– 消耗的功率 ：机器人从起始状态移动到目标状态所消耗的总功率 $ P $ 计算如下：

$$
P = \sum_{i} \frac{f_i d_i}{\Delta t_i},
$$

其中 $ f_i $、$ d_i $ 和 $ \Delta t_i $ 分别为施加的控制力、产生的位移矢量 $ d_i $ 和持续时间。

– 平滑度 S ：轨迹的平滑度 $ S $ 可通过加加速度（即加速度对时间的导数）来衡量：

$$
J(t) = \frac{d a(t)}{dt}.
$$

对于给定的轨迹 $ \tau $，平滑度 $ S $ 定义为沿 $ \tau $ 的加加速度平方和：

$$
S = \int_{t_i}^{t_f} J(t)^2 dt,
$$

其中 $ t_i $ 和 $ t_f $ 分别为初始时间 $ t_i $ 和结束时间。

– 规划时间 ：本体论基于物理的规划器计算解轨迹所消耗的总时间。

– 成功率 ：根据成功运行次数计算。

4 结果与讨论

4.1 仿真设置

仿真设置包括一个机器人（绿色球体）、自由可操作物体（蓝色立方体）、约束导向可操作物体（紫色立方体）以及固定物体（三棱柱、墙壁和地板）。基准测试在图1所示的三个不同场景中进行，这些场景根据杂乱程度进行区分。图中显示了机器人的初始构型，目标机器人构型以黄色圆圈表示。图1‐a描述了最简单的场景，包含一个机器人、自由可操作物体和固定物体。第二个场景如图1‐b所示，包含一个机器人、自由可操作物体、固定物体以及一个约束导向物体。在此场景中，狭窄通道被一个约束导向可操作物体（只能沿y轴方向垂直推动）占据，机器人必须将其推开才能清理通往目标的路径。需要注意的是，由于从起始状态到目标状态不存在无碰撞路径，因此几何规划器和动力学规划器均无法计算出路径；只有基于物理的规划器能够通过推动物体来计算路径。最后一个场景如图1‐c所示，具有最高的杂乱程度。目标区域被一个约束导向可操作物体（只能沿x轴方向水平推动）占据；为了到达目标区域，机器人需要通过推动物体将其清空。同样，此处也不存在无碰撞路径。所有规划器均使用相同的规划参数：目标偏向等于0.05，采样控制范围在‐10N到10N之间，传播步长等于0.07s。

图1 基准测试所用的仿真设置。机器人（绿色球体）位于起始状态，黄色圆圈表示目标位置。

4.2 基准测试结果

对于每个场景以及第2节中总结的每种动力学运动规划器，本体论基于物理的运动规划器均运行10次。基准测试参数的平均值以直方图形式呈现。图2展示了使用KPIECE的本体论基于物理的运动规划器的一次示例执行的一系列快照。为了估计构型空间的覆盖率和解轨迹，图3描绘了构型空间以及不同动力学规划器计算出的解路径。图4显示了平均规划时间（最大允许规划时间设置为500秒）。除EST外，所有规划器均能在最大规划时间内计算出解。在所有规划器中，KPIECE计算解的效率最高。规划器的成功率如图5所示。每个场景的成功率基于成功运行次数（即规划器在时间上限内计算出解的次数）分别计算，然后通过计算三个场景的平均成功运行次数来确定每个规划器的平均成功率。结果表明，KPIECE的整体成功率最高。SyCLoP‐RRT也表现出令人印象深刻的成功率，而EST的成功率为零。

动力学参数、作用量和功率的结果分别如图6和图7所示。关于作用量，具有最小作用量的解轨迹被视为最合适的解。在给定时间内计算出解的规划器中，平均而言，KPIECE的作用量最小，SyCLoP‐RRT的作用量最大。关于功率，期望机器人沿解轨迹运动时消耗的功率最小。我们的分析表明，SyCLoP‐RRT能够找到功率最优轨迹，而KPIECE表现最差。RRT和SyCLoP‐EST的结果几乎相同。关于轨迹平滑度，比较结果如图8所示。SyCLoP‐RRT在所有规划器中计算出的轨迹平滑度最高，而KPIECE在平滑度方面的表现最差。由于EST未能在规定时间内计算出解，因此EST的作用量、功率P和平滑度S被设为无穷大，且未在直方图中显示。

图2 使用本体物理运动规划器执行的一系列快照。

图3 构型空间和解路径：每一行对应一个场景，每行中的各列（从左到右）分别表示使用KPIECE、RRT、SyCLoP‐RRT和SyCLoP‐EST得到的构型空间和解路径。

图4 每个场景的平均规划时间（10次运行）以及总体平均的规划时间（三个场景）。

图5 各场景下规划器的成功率及总体平均成功率（三个场景）。

图6 每个场景的平均动作量以及总体平均值（三个场景）。

图7 机器人沿解路径移动时的平均消耗功率及总体平均值（三个场景）。

图8 每个场景的平滑度度量及总体平均值（三个场景）。

4.3 讨论

我们提出了一种用于基于物理的规划的基准测试标准。基于该提出的准则，使用不同的动力学运动规划器对基于物理的规划的性能进行了评估。我们的分析表明，在规划时间、成功率和作用量方面，KPIECE是最合适的规划器。SyCLoP‐RRT在计算出的轨迹平滑度S和机器人沿计算出的路径移动时消耗的功率P方面，表现出显著的结果。SyCLoP‐RRT的规划时间和成功率也令人印象深刻。RRT在整个评估中表现一般。SyCLoP‐EST在作用量和功耗方面表现良好，但其规划时间值非常高，且成功率较低。EST未能在规定时间内计算出解，成功率为零。

5 结论与未来工作

本文提出了一种针对基于物理的运动规划器的评估标准。所提出的基准测试标准通过计算解路径上的动力学参数（例如机器人沿解路径移动时消耗的功率）来评估计算出的解路径的质量。此外，基于所提出的基准测试标准，对本体论基于物理的运动规划器（使用不同的动力学运动规划器）的性能进行了评估，并详细讨论了每种动力学运动规划器的计算出的属性。目前，该评估标准仅在简单场景中实现，并以推动动作作为唯一的操作动作；未来工作将把所提出的基准测试标准应用于移动操作器，以同时对抓取和拾取与放置操作动作进行基准测试。