基于可达性的无环接触规划

一种基于可达性的杂乱环境中的无环接触序列规划器

S. 图诺, N. Mansard, C. Park, D. Manocha, F. Multon 和 J. Pettré

1 引言

我们研究在杂乱环境中规划多足机器人无环接触序列的问题。无环接触规划是运动规划的一个特定类别，其中生成轨迹的每个构型都必须与环境保持接触，以维持系统的平衡。该问题的难点既体现在实际中由于靠近障碍物（导致有效构型的采样十分繁琐），也体现在理论上构型空间的叶状结构，其中零测度流形以组合方式相交[20]。无环运动规划是机器人学、神经科学和生物力学领域中的一个重要问题，同时在虚拟角色动画中也具有研究价值。该领域的早期研究主要依赖于运动图的局部调整[13]或专门设计的运动控制器[18]。这些方法本质上无法适应新情况，或在不可预见的环境中发现复杂的行为。在机器人学领域，学术界最初关注的是周期性运动模式的生成，特别是双足行走在平坦地形上的情况[12]。尽管目前周期性双足接触的规划已经相当成熟，并且已有实时解决方案[3]，但对于更通用的非周期性接触问题，该问题仍然未得到解决。

1.1 研究现状

非周期性接触的规划问题最早由Bretl等在[6]中进行了完整描述，其中证明该问题需要同时处理两个子问题：P1：机器人根部在SE(3)空间中的引导轨迹；以及P2：沿该轨迹规划一个非周期性、平衡接触构型的离散序列。

一个关键问题是在同时考虑可能的接触构型和潜在轨迹时避免组合爆炸。这篇开创性论文提出了一种初步有效的算法，能够处理简单情况（例如攀爬场景），但在任意环境中不具备可扩展性。在此之后，多篇论文将该方法应用于特定场景，通常通过设定固定的可能接触集来限制组合复杂性（[11, 21]）。

以下大多数论文探讨了处理组合复杂性的替代表述。主要探索了两个方向。一方面，对根轨迹P1和接触位置P2进行局部优化，以将完整问题的组合复杂性转化为微分复杂度，但代价是存在局部收敛[14, 15]。为了保持合理的计算时间，该方法对机器人模型采用简化动力学模型。尽管如此，计算时间仍远未达到交互式水平（约20个接触序列需要1分钟的计算时间）。

Deits等人[8]将该问题视为混合整数问题，但仅考虑了循环的双足运动。除了计算成本外，这些基于优化的方法的一个主要缺点是，在应用于无环接触规划时，它们仅能提供局部收敛。

另一方面，两个问题 P1和 P2可能被解耦以降低复杂性。解耦的优势首先通过手动设置一条粗略的引导轨迹（即 P1 的一种特设解决方案）得到验证[10]。P2 随后被处理为在引导轨迹邻域内组合计算可行接触序列的问题。尽管如此，该方法仍需付出难以承受的计算时间（数小时）才能找到解决方案。此外，该方法是次优的，因为运动的质量取决于引导轨迹的相关性，而该相关性并未被先验评估。Bouyarmane等[4]致力于通过将轨迹的关键帧扩展为全身、平衡接触构型，自动计算具有接触可行性保证的引导轨迹。

随机使用广义逆运动学求解器将构型投影到接触子流形上，这是一个计算代价高的过程（在所给出的示例中，计算一条引导轨迹大约需要15分钟）。此外，这种显式投影仍然只是一个充分条件，并不能对轨迹中两个关键位置之间路径的可行性提供强有力的保证。

1.2 论文贡献与结构

我们选择专注于基于采样的方法，这种方法在杂乱环境中更有能力找到复杂的轨迹。尽管该问题的理论结构已被充分理解，但目前尚无解决它的可扩展的方法。原始问题的组合复杂性过高，以至于直接解决它毫无希望。需要采用替代表述来获得实际解决方案。我们相信，将引导轨迹与接触序列分离是最有前景的方向[10]。然而，这一方向提出了两个尚未解决甚至尚未被恰当表述的理论问题：

• 引导轨迹必须满足一个性质，以确保存在用于驱动该轨迹的接触序列。在无环规划中，该性质尚未被研究：验证轨迹的唯一方法是显式计算所有接触，这在计算上并不可行[4]。
• 对于给定的根部轨迹，存在无限多种可能的接触序列组合。针对循环情况，已对具有特定性质（如最小接触变化次数、鲁棒性、效率或自然性）的某一特定接触序列的选择进行了研究，但该方法尚未被有效应用于杂乱环境（先前的研究大多随机选择一种接触序列，可能导致非常繁琐的接触序列）。

我们声称，文献[4]中提出的引导轨迹的期望的接触特性可以在一个更低维度的空间中进行表述，我们将该空间称为Creach。这种表述方式可在计算上更高效地规划引导轨迹，同时提供与直接在构型空间中规划等效的保证。在Creach空间中规划时获得的特定性质中，我们希望保证任何降维轨迹实际上都能导致一个可行的接触序列；在这种情况下，我们称该降维轨迹是真正可行的。理论上有可能确保任意降维轨迹都是真正可行的，尽管在实际中近似实现这一性质更为高效。一旦引导轨迹具备真正可行性，我们便可以专注于选择某一个特定的接触序列，例如使序列中的接触数量最少，或最大化机器人效率或风格的序列。

请求
A
RB-PRM
B
EFORT

基于这些基本观察，我们实现了一种非常高效的非循环接触规划器。我们的方法基于概率路线图（PRM），用于离线计算近似真正可行的引导轨迹。然后，规划器通过优化从PRM中生成的引导轨迹，在线求解接触序列。

该规划器能够在几秒内为复杂系统（如人形机器人，28个关节；昆虫形机器人，48个关节）在攀爬等经典场景中计算出物理一致的接触序列，对于从损坏的卡车中撤离等极为复杂的任务也耗时不到一分钟。此外，该规划器还可推广至灵巧操作动作的规划，初步结果已验证了其可行性。

本文的贡献有两点。我们提出了对当前最高效的基于采样的非循环接触规划实践方法的首个理论表征。并且基于该表征，我们提出了一种非常高效且通用的非循环接触规划器实现方法，这是首个兼容交互式应用的实现。

我们提出了一种框架，用于解决多足机器人在杂乱环境中的运动规划问题：给定起始和目标构型，目标是计算一个接触构型序列以实现该运动。例如，我们可以考虑图1右侧所示的站立任务。该问题被分解为两个顺序阶段：（1）机器人根部引导轨迹的计算；（2）沿该轨迹实现运动的离散接触构型序列的计算。本节其余部分将在第2节中介绍我们方法的整体结构。接下来的第2和 3节分别针对问题P1和P2提出我们的解决方案。最后，我们在各种场景下对规划器进行了完整的实验验证，使用了三种截然不同的运动链（人形机器人、昆虫形机器人和三指操作器）。

1.3 引导轨迹的计算

我们首先考虑规划相关引导轨迹的问题。目标是计算一条根位姿的轨迹，以实现接触创建。这一部分的目标是保持规划器的完备性：它应该能够探索任何可能的引导轨迹，但同时，任何计算出的引导轨迹必须是真正可行的，即必须导致一个有效接触序列。

此类根位姿的直观描述是“接近但不太接近”：接近，是因为接触面必须部分位于机器人的运动范围内（如图2–1中右臂所示）；不太接近，是因为机器人必须避免碰撞（如图2–1中包含躯干的包络所示）。我们正式定义 Creach为感兴趣根位姿的集合，在该集合中我们使用基于采样的规划器——可达性PRM–RB-PRM–来计算引导轨迹（见图1–a）。在Creach中的规划归结为在SE(3)中的规划，其具有可接受的实际复杂度。详细内容见第2节。

1.4 生成接触构型的离散序列

第二阶段是将引导轨迹扩展为一系列接触构型（图1–b）。得益于我们为引导轨迹所获得的良好特性，生成一个随机的接触序列成为一个简单的问题。

本部分的目标是选择一个高效的序列，特别是通过减少序列中的接触数量来提高效率。为了高效地创建接触，我们将每个肢体视为连接到根部的操纵器，并从预先计算的构型数据库中选择最相关的接触（图2）。相关性被定义为对机器人准静态平衡的贡献以及对根部运动的贡献。该任务效率基于扩展力传递比（EFORT）[22]进行衡量。详细内容见第3节。

1.5 符号约定与定义

向量x用粗体小写字母表示。矩阵A用粗体大写字母表示。集合C用大写斜体字母表示。标量变量和函数用小写斜体字母表示，例如r或f(x)。

机器人是一个运动学链 R，包含 n+ 6个自由度（DOF）。R由 l个肢体 Rk, 1 ≤ k ≤ l 组成，这些肢体连接到一个根部。它由一个构型 q ∈ SE(3)× Rn 描述。我们定义了 q 的一些相关投影：

• qk表示肢体Rk的构型（关节值向量）；
• qk表示与不相关的R的关节值向量Rk。为方便起见，我们定义q= qk⊕qk；
• q0 ∈ SE(3)表示机器人根部R的位置和姿态。

环境O定义为它所包含的障碍物Oi的并集。机器人躯干所占据的体积记为W0（图3右侧：中心圆柱体）。肢体Rk的运动范围记为Wk（图3右侧：四个椭圆）。

$$
W_k = {x \in \mathbb{R}^3: \exists q_k, p_k(q_k) = x}
$$

其中pk表示Rk（仅平移）的末端执行器位置，对应于q0的零位移。我们还定义了W = ⋃ₖ Wₖ。最后，我们将Wk (q₀), 0<k ≤l定义为体积Wk经刚体位移q0平移和旋转后的结果。

2 在Creach中计算引导轨迹（阶段1）

我们考虑计算一个多足机器人几何根部的引导轨迹q₀(t): [0, 1] → SE(3)，以连接起始构型和目标构型。如前一节所述，目标是确保引导轨迹中的任意构型q₀都是真正可行的，即可被映射到一个处于接触状态的平衡构型。我们用Ccontact ⊂ SE(3) × Rⁿ表示机器人的接触子流形。

我们说一个根部位姿 q₀是真正可行的，如果存在一个双射映射π使得

$$
\pi: q_0 \in SE(3) \rightarrow q_0 \oplus q_0 \in C_{\text{contact}}
$$

所有真正可行的根位姿的集合记为 Creach。相应地，若 ∀t ∈[0, 1], q₀(t) ∈ Creach，则轨迹 q₀(t)是真正可行的。

为了使两阶段无环接触规划器具有精确且完备性，我们需要对引导轨迹生成器施加两个条件的组合：所有生成的轨迹必须是真正可行的（充分条件）；引导规划器必须是完备的，即它必须能够发现任何真正可行的轨迹（必要条件）。

2.1 真正可行性条件

默认情况下，真正可行性意味着通过展示一个有效的 π 来实现构造性证明。这是[4]所采用的方法。然而，在实践中计算一个有效的 q₀ 是昂贵的。在本节中，我们转而定义真正可行性的必要条件和充分条件，这些条件不需要这种显式计算。

真正可行性：必要条件
要使接触成为可能，体积 Oᵢ ∈ O必须与运动范围 W(q₀)相交（图 2–1）。此外，如果 q₀是真正可行的，则机器人躯干 W₀(q₀)必然不会与环境 O发生碰撞。

因此，我们可以用一组满足可达性条件的 Creach 来近似 Creach ⊂ C₁reach，其可达性条件定义为：

$$
C_1^{\text{reach}} = {q_0: W(q_0) \cap O \neq \emptyset \land W_0(q_0) \cap O = \emptyset}
$$

很容易证明Creach ⊂ C₁reach（根据包含集的构造方式）。这一包含关系非常重要：它直接意味着任何在SE(3) × Rⁿ中具有完备性保证的运动规划算法，在 Creach × Rⁿ中也是完备的。这是一种对搜索空间的强有力缩减，可直接应用于任何现有方法。

条件C₁reach仅为必要条件，这意味着一种这样的根部放置可能并非真正可行：在实践中，无法保证为C₁reach中的每条引导轨迹找到有效接触序列。

真正可行性的充分条件
真正可行性的一个平凡充分条件可以通过对C₁reach进行修改来构造，即将 W₀ 替换为包含整个物体的包围体积Bmax。我们用C∞reach ⊂ Creach表示对应于充分条件的根位姿集合。

通常情况下，这种包含关系是严格的，这意味着我们会失去两阶段接触规划器的完备性（即规划器无法发现Creach\C∞reach内的轨迹）。然而，充分条件保证了任何此类轨迹都会产生一个有效接触序列（即π被定义）。

2.2 可达性：一种折衷条件

充分条件在实践中并不理想，因为它会导致求解器丢失太多有意义的轨迹。必要条件同样不完美，因为规划器的第一阶段可能会停留在实际中并不可行的引导路径上。虽然有可能找到一个既必要又充分的形状B，但直观来看，这种情况通常极不可能出现。构造一个能够产生必要且充分条件的形状W₀ ⊂ B ⊂ Bmax（或证明其不存在）超出了本研究的范围。

然而，在W₀和Bmax之间，可以在必要条件与充分条件之间找到一种折衷。我们将W₀s定义为体积W₀，其经过一个缩放因子为s ∈ ℝ⁺的缩放变换。然后我们考虑空间Csreach

$$
C_s^{\text{reach}} = {q_0: W(q_0) \cap O \neq \emptyset \land W_0^s(q_0) \cap O = \emptyset}
$$

s越高，可达性条件越接近充分；如果s= 1，则规划器是完备的。对s的参数化可以在这两个理想特性之间找到折衷。第4节表明，在实践中，很容易调整s以保留大多数有意义的引导轨迹，同时避免引入错误的引导轨迹。

2.3 在Csreach中使用RB-PRM计算引导轨迹

一旦固定了s的值，就可以使用任何基于采样的运动规划器在Csreach中规划轨迹。唯一的区别在于，验证构型及相关局部路径的有效性时，需将传统的碰撞检测方法替换为测试是否属于Csreach。因此，无需为RB-PRM提供伪代码，尽管我们在此提供了关于自身实现的相关信息。

我们选择将RB-PRM实现为可见性PRM[16]的一种变体，这通常会比经典PRM规划器产生更少的节点。相关的缺点是规划器返回的路径可能不是最短路径，这在高度杂乱的环境中通常不是问题。为了更高效地采样Csreach的构型，我们使采样过程偏向于生成靠近障碍物的构型，类似于[1]在狭窄通道中生成构型的方法。首先，将一个构型设置为某个障碍物表面上的随机点。然后对该构型进行随机平移和旋转，直到满足可达性条件为止。同样，我们的实现唯一的区别在于对Csreach进行采样，而不是对Cfree进行采样。

3 从引导轨迹到离散接触构型序列（阶段2）

作为本阶段的输入，我们考虑一条针对机器人根部q₀(t):[0, 1] → SE(3)的真正可行的引导轨迹R。现在我们考虑计算机器人肢体轨迹的第二个问题q₀(t)。由于我们假设具有真正可行性，因此我们知道这样的轨迹存在。与以往的研究不同，此处的目标不是找到任意一条这样的轨迹，而是选择一条具备特定性质的轨迹。

具体而言，我们在此展示如何构建一个接触变化次数较少且姿态具有高效率和自然性的接触序列。

更准确地说，（2）中引入的任意映射 π 均可用于将 q₀（t）扩展为全身轨迹。本文提出一种特定的 π构造方法，可生成有趣的接触序列。

3.1 引导轨迹的扩展

引导轨迹 q₀(t) 首先被离散化为一系列 j 关键位姿 :

$$
Q_0 = [q_0^0, …, q_0^{j-1}]
$$

其中 q₀⁰ 和 q₀ʲ⁻¹ 分别对应起始和目标构型。为了确保接触转换阶段的连续性，我们将 π 重写为以下递归形式：

$$
\pi: {Q_0 \in \text{Creach} \rightarrow C_{\text{contact}} \
q_0^i \rightarrow q_0^i \oplus g(q_{i-1}, q_0^i)}
$$

我们通过 π(q₀^i)独立定义为gₖ。在定义gₖ时，必须考虑两个方面：肢体Rk是否处于接触状态？以及它正在优化哪个准则？

保持接触 ：如果可能，肢体在时刻i −1的接触将在i时继续保持。若逆运动学求解器无法找到满足关节限位的无碰撞肢体构型，则接触将被中断[2]。如果求解器失败，则接触被断开，并为肢体分配一个无碰撞的构型。

一旦为所有肢体选择了一个初始候选构型，就通过重力拧是否位于重力-惯性锥内（即是否存在有效的接触力来补偿机器人重量）来检验准静态平衡，该方法采用中所述的几何方法[19]。如果未达到平衡，则使用以下过程随机生成新的接触（图4）。

创建接触 ：我们考虑一种构型，其中一些肢体处于接触状态，一些肢体自由，且未强制执行准静态平衡。为了实现平衡，我们按以下方式进行：随机选择一个无接触肢体；如果没有无接触肢体，则选择最先发生接触的肢体。使用[22]中引入的接触生成器，我们将该肢体的构型投影到能够增强平衡的接触上（如果存在这样的接触）（图5）；如果无法实现平衡但可能发生接触，则仍然生成该接触；如果仍未实现平衡，则选择下一个肢体并将其投影到接触构型，依此类推。这种方法可能导致现有接触的重新定位，在这种情况下会插入中间状态以重新调整接触。当前的实现不能保证规划器一定能生成平衡构型，因为真正可行性并未完全保证。然而在实践中，如第4.2节所述，规划器在大多数情况下是成功的。

4 结果

我们规划器的主要优势在于它能高效地适用于任意机器人形状。我们首先通过在五个具有挑战性的场景中使用三种截然不同的机器人（人形机器人、昆虫形机器人、灵巧手）生成大量多样的运动来验证这一特性。本文从两个方面对方法进行了评估：定性方面，通过配套视频展示接触序列的自然性；定量方面，通过统计测量折衷条件（第4.2节）的有效性以及算法的性能（第4.3节）。

4.1 机器人模型与场景

图6描述了实验中使用的机器人。人形机器人有四个肢体，每个肢体具有7个自由度，总共具有34个自由度。昆虫形机器人有六个肢体，每个肢体具有8个自由度，总共具有54个自由度。该手部有三个手指，每个手指具有6个自由度，总共具有24个自由度。

在所有考虑的场景中，问题的表述始终相同：将起始与目标构型作为场景的输入，该框架输出一个连接起始和目标构型的静态平衡接触构型序列。

配套视频（可在 http://youtu.be/LmLAHgGQJGA 查看）展示了在所有这些场景中获得的完整接触序列。该视频仅呈现接触构型（不包含接触之间的插值，因为这超出了本文的范围）。

卡车撤离（人形机器人和昆虫形机器人） ：机器人的车门被阻塞，必须从前窗爬出。图7展示了为两种机器人获得的接触序列：RB-PRM能够在具有狭窄通道的高度杂乱环境中找到解决方案。

攀爬（人形机器人和昆虫形机器人） ：机器人必须在具有多个抓握点的墙壁上进行攀爬。在此场景中，我们对采样的根节点位置提出了更严格的要求：要求多个运动范围 Wk 发生碰撞。图8展示了为人形机器人获得的接触序列。

钢笔操作（三指手） ：该场景旨在说明我们方法的通用性：我们考虑机械手的一个操作任务，并使用我们的接触规划器为手指（被视为执行器）计算引导轨迹（图9）。尽管我们未解决考虑滚动运动这一难题，但该规划器能够在不到5秒的时间内计算出所示序列。

其他场景（人形机器人） ：站立场景（已在图1中展示）取自[10]：它对应于构型空间中的一个狭长通道。在配套视频展示的蹲伏场景中，角色自动从站立变为蹲伏姿势，以爬行通过障碍物下方。

4.2 可达性条件的参数化

为了找到合适的 Csreach 来采样引导轨迹，我们计算了在最复杂的卡车场景中，每个机器人在不同 s取值下的拒绝率。对于给定的 s，10⁶根位置和方向在 Csreach 中计算。在每种情况下，我们尝试生成一个无碰撞接触构型，数据库包含每个肢体的 N= 10⁵个采样构型。拒绝率是失败次数与试验次数的比值。

根据图10，s 被经验选择为拒绝率达到最小值时的最小值。对于人形机器人，我们选择 s= 2.2；对于昆虫，选择 s= 2.8。

4.3 性能

每个肢体用于生成接触的采样数量为10000。表1列出了每种场景下规划器各阶段所花费的平均时间（秒）以及序列中的接触阶段数量。

我们观察到昆虫需要大量的接触，这可以通过其受限的运动范围来解释。生成导航图所花费的时间约为一分钟。尽管攀爬场景的环境相对开放，但生成其导航图所花费的时间仍然较长，这是由于可达性条件施加了额外的限制。难以找到平衡构型的问题在本质上影响了生成接触所花费的时间。

序列中的接触数量大致估计了其持续时间（秒）。除了机器人从卡车上爬出的情况外，所有接触生成均为实时。除了视频中展示的生成轨迹的质量外，这些计算时间也是一个主要的实际成果。

	生成 RB-PRM (离线)	生成接触序列	接触数量
卡车撤离 （人形）	73	15	10
卡车撤离 （昆虫形）	70	23	48
攀爬 （人形）	25	5	15
攀爬 （昆虫形）	21	27	51
蹲伏 （人形）	5	6	22

5 讨论与未来工作

本文考虑在杂乱环境中进行无环接触规划，将其表述为两个依次解决的子问题：P1：计算机器人根部的引导轨迹，该轨迹可被扩展；P2：沿此轨迹计算离散接触序列。我们对P1的贡献在于刻画了引导轨迹必须满足的性质，特别是确保非循环接触规划器的完备性。我们引入了一个低维空间Creach，该空间可映射到机器人的接触子流形，并通过我们的基于可达性的规划器进行近似和高效采样。我们对P2的贡献是一种实用的接触生成方案，能够考虑用于增强接触有趣特性的准则。其中一个准则EFORT用于演示该方法，并优化与运动方向相容的力施加。

除了理论贡献之外，我们的结果表明，该方法能够在三个难以协调的准则之间实现折衷：通用性、性能和解的质量，使其成为首个兼容交互式应用的无环接触规划器。关于通用性，可达性条件结合基于肢体分解的方法，使得该方法能够适用于任意多足机器人。唯一的先决条件是指定体积W0，其可通过类似第4.2节中进行的统计分析进行调整。关于性能，我们的框架在处理P1或P2时优于现有方法，从而在静态已知环境中实现了接近实时的计算成本。关于轨迹质量，通过对可达性条件的参数化，我们能够在所有提出的场景中计算出合理的轨迹，并且拒绝率较低。至于[4]仍可能发生失败，这是由于在计算引导轨迹时采用了折衷准则所致。

未来工作将专注于对Creach进行更精确的公式化，以解决这一局限性。尽管存在这些局限性，我们仍成功在真实的HRP-2机器人上实现了完整运动的生成，并在计算出的接触序列之间进行插值，且具备实时性能[7]。我们当前的目标是制定改进的启发式方法，以确保规划器在静态平衡[9]和姿态之间的过渡方面的鲁棒性。