论文笔记（八十一）Online Replanning with Human-in-The-Loop ... A Trajectory Optimization based Approach

文章概括

引用：

@article{papallas2020online,
  title={Online replanning with human-in-the-loop for non-prehensile manipulation in clutter—a trajectory optimization based approach},
  author={Papallas, Rafael and Cohn, Anthony G and Dogar, Mehmet R},
  journal={IEEE Robotics and Automation Letters},
  volume={5},
  number={4},
  pages={5377--5384},
  year={2020},
  publisher={IEEE}
}

Papallas, R., Cohn, A.G. and Dogar, M.R., 2020. Online replanning with human-in-the-loop for non-prehensile manipulation in clutter—a trajectory optimization based approach. IEEE Robotics and Automation Letters, 5(4), pp.5377-5384.

主页：
原文： https://ieeexplore.ieee.org/abstract/document/9131745
代码、数据和视频：

系列文章：
请在 $《$文章$》$ 专栏中查找

---

宇宙声明！

引用解析部分属于自我理解补充，如有错误可以评论讨论然后改正！

---

摘要

我们关注的问题是在一个模拟仓库环境中，多台机器人并行尝试解决穿越杂物的到达（reaching through clutter）问题。在这样的环境下，我们研究了通过引入人机交互（human-in-the-loop）为机器人规划器提供指导所能带来的性能提升。这些操作问题对自主规划器而言极具挑战性，因为它们必须在高维空间中搜索解决方案。此外，物理模拟器存在不确定性问题：在仿真中有效的轨迹在现实世界执行时可能会失效。为了解决这些问题，我们提出了一种带有人机交互的在线重规划方法。该系统使机器人能够自主规划并执行轨迹，同时在执行过程中任何需要时向人类操作员寻求高层次建议。该方法旨在将所需的人力降至最低，从而增加单个操作员可并行指导的机器人数量。我们在仿真环境和真实机器人上进行了实验，分别由一位经验丰富的操作员和一位新手操作员参与。我们的结果表明，在模拟仓库场景中使用我们的方法并同时运行六台机器人时，性能显著提升。

关键词——人因与人机交互；操作规划；优化与最优控制。

I. 引言

设想这样一个场景：大量机器人在仓库中工作，试图从杂乱的货架上取物品。我们研究了在人机交互环节中引入人类操作员是否能够提升此类系统的性能，并提出了一种规划与控制方法以实现这一目标。

以图 1 为例，其中每一行展示了不同的机器人。横轴表示时间。每个机器人的任务是通过移开障碍物来伸到货架上抓取绿色标示的物体。有一个人类操作员可以通过建议移动某些物体来提供帮助。最初，在每一行的第 1 帧中，所有机器人都尝试使用轨迹优化来生成规划。其中一些机器人，例如 $r1$ 和 $rn$，能够快速生成可行轨迹并开始自主执行，而无需任何人类帮助。由于物体运动具有不确定性，机器人会执行在线重规划（类似模型预测控制），即在每个时间步对轨迹进行重新优化并执行。然而，机器人 $r2$ 在第 1 帧就决定请求人类帮助，并向操作员发出提示。在第 2 帧中，人类操作员与机器人 $r2$ 互动，快速检查场景，通过界面提供高层次输入（图中白色箭头所示），然后脱离参与。在第 4 帧中，机器人 $r2$ 再次尝试生成轨迹，这次利用了人类提供的输入，然后继续使用在线重规划进行自主执行。与此同时，在一段自主执行后，$rn$ 所在环境中的物体运动与规划器的预期大相径庭，导致 $rn$ 也需要人类帮助。操作员接到提示提供输入后，执行继续进行。

图1 所示为所提出的系统。每一行展示了一个并行工作的不同机器人。只有在需要时才会请求人类输入（蓝色）。人类的高层次输入用白色箭头表示。规划用绿色表示，执行用红色表示。

这样的系统具有若干优势。其中一大优势是在规划非握持方式的穿越杂物取物运动时可以获得人类帮助。最近已有多种自主规划器被提出用于解决这一问题[1]–[11]，但在问题较困难的实例中，成功率仍然较低，或者规划时间长达数十秒甚至数分钟。这在一定程度上解释了为何当前工业应用仅限于机器人能够直接伸手取物、无需与其他物体交互的场景。在本工作中，我们研究了通过引入人机交互提供指导所能实现的性能提升，特别是通过将人力付出降至最低，从而增加单个人类操作员能够并行指导的机器人数量。此类高层次指导对于人类而言通常简单且自然，并能显著加速规划器在复杂场景中的性能。

上述系统的另一个优势是使用在线重规划。当机器人执行非握持计划时，现实世界中的物体运动与模型预测不同，因此必须更新计划。基于轨迹优化的规划方法在此类场景中尤为有效，因为新的优化循环可以使用先前的解进行热启动，并且在少量优化迭代中收敛。在这方面，我们方法的一个关键创新是在此在线重规划架构中集成人机交互，使系统能够在执行过程中的任何时刻使用人类帮助，而不仅仅作为规划问题的初始输入。我们之前的工作使用人类指导来解决穿越杂物的到达问题[12]，但该方法要求在规划之前提供人类输入。它还使用基于采样的规划方法（与我们在本信中使用的轨迹优化方法相对）。然而，轨迹优化更易于在线重规划，因为可以使用先前迭代得到的轨迹来热启动优化。

因此，我们所提出系统的一个关键新特性是使用轨迹优化并结合人类输入执行轨迹优化。 为此，我们提出将人类输入作为目标/代价函数的一部分，由优化器进行最小化。这使得人类输入能够在在线重规划过程的每个步骤中轻松集成到优化中。

我们所提出系统的最后一个创新特性是对人类时间的高效利用。我们之前的工作 [12] 无论自主规划器能否高效解决问题，都需要人类帮助。我们建议仅在自主规划器预计会失败或在预期人类帮助能够显著加快规划时，才招募人类提供帮助。这样的系统在性能上应至少与最先进的全自主系统相当，并在需要时额外具有人类介入的机制。 我们提出了两种不同的方法来实现这一点。第一种方法是在规划在固定时间内未能生成计划时请求人类帮助。第二种方法与轨迹优化更好地集成，在优化陷入局部极小值时请求人类帮助。

为了评估所提出的系统，我们进行了多种不同的实验。首先，在 V-B 节中，我们评估了两种请求人类帮助的方法，并将它们与两种自主方法进行了比较。我们分别与一位经验丰富的用户和一位新手用户进行了实验。在 V-C 节中，我们在带有人为不确定性的仿真环境和真实机器人（图 2）上进行了实验，以检验我们的在线重规划执行机制的鲁棒性。最后，在 V-D 节中，我们在仿真中测试了整个系统：一名人类操作员同时指导六台机器人同时解决多个规划问题，并将其与并行全自主规划器进行了比较。

因此，本工作的创新贡献如下：
（1）将人类输入集成到非握持操作的优化问题中；
（2）我们的框架允许人类操作员同时管理多台机器人进行非握持操作；
（3）我们使用自适应方法根据问题难度决定何时请求人类输入；
（4）我们提出了一种鲁棒的执行方法以应对现实世界的物理不确定性。

II. 相关工作

Amazon 挑拣挑战赛 [13] 是一项鼓励在杂乱环境中实现全自主取物解决方案的比赛。该比赛因其对仓库机器人操作潜在的近期影响而受到特别关注。比赛表明，即使在假设条件放宽的情况下，对于全自主系统而言，这些问题仍然很困难，尤其是当目标物体被杂物遮挡时。

针对穿越杂物到达问题的自主解决方案可分为三类：基于采样的规划方法 [5]、[6]、[9]，基于轨迹优化的方法 [3]、[14]，以及基于学习的方法 [4]、[7]、[15]、[16]。虽然这些方法在不同程度上取得了一定的成功，但由于问题的高维和欠驱动特性，以及真实物理不确定性的影响，困难实例对自主系统而言仍具有挑战性。我们采用受 STOMP 和 CHOMP 启发的轨迹优化方法 [17]、[18]，但将其应用于含物理仿真的非握持式杂物操作，并且扩展研究了如何以在线重规划的方式将人类输入与轨迹优化集成，以及如何让轨迹优化过程决定何时请求帮助。

Williams 等人 [19] 描述了一种模型预测路径积分（MPPI）方法，用于跟踪已优化轨迹。他们在现实世界执行每一步后运行一次优化迭代。我们采用了类似的方法，但应用于非握持式操作场景。此外，我们的每次优化步骤都包含到达目标状态的约束（例如，使目标物体进入机器人手中），而不仅仅是优化一个软代价。

使用高层次输入的思路与最近的机器人分层规划 [2]、[20]–[23] 和任务与运动规划（TAMP）[24]–[26] 等工作相关。这一系列工作表明，若能获得良好的任务高层规划，可将低层运动规划器的搜索引导到相关但受限的搜索空间，从而加快规划速度并提高成功率。受已有的人机交互系统研究启发 [27]–[40]，本信中我们探讨了利用人类操作员来建议此类高层规划的可能性。然而，上述大多数人机交互规划工作侧重于向规划器提供线索，引导其穿越无碰撞空间；而我们关注的是非握持式操作。我们之前的工作 [12] 提出了一种使用人类指导的非握持式操作规划器，但与当前工作在规划方法（轨迹优化 vs. 基于采样）、执行方式（闭环在线重规划 vs. 开环执行）以及最重要的人类输入请求方式（仅在需要时请求 vs. 对所有问题均请求）方面存在显著差异。

III. 问题表述

我们的环境包括一台机器人 $r$、$\mathcal{O}$ 个机器人可交互的可移动障碍物，以及机器人不应交互的其它静态障碍物。我们还有一个要到达的目标物体 $o_g\in \mathcal{O}$。

机器人的状态记为 $x^r=(q^r,{\dot{q}}^r)\in {\mathcal{X}}^r$。$q^r\subset SE(2)$ 是机器人的配置，${\dot{q}}^r$ 是机器人的速度。同样地，我们将可移动障碍物 i ∈ { 1 , … , ∣ O ∣ } i\in\{1,\dots,|\mathcal{O}|\} i∈{1,…,∣O∣} 的状态记为 $x^i=(q^i,{\dot{q}}^i,{\ddot{q}}^i)\in {\mathcal{X}}^i$，其中 $q^i\subset SE(2)$ 是物体的配置，${\dot{q}}^i$ 和 ${\ddot{q}}^i$ 分别是物体的速度和加速度。整个系统的状态空间 ${\mathcal{X}}^E$ 由以下笛卡尔积给出：
$${\mathcal{X}}^E={\mathcal{X}}^r\times {\mathcal{X}}^1\times {\mathcal{X}}^2\times \cdots \times {\mathcal{X}}^{|\mathcal{O}|}.$$

一、模型中各要素的含义

1. 机器人状态 $x^r=(q^r,{\dot{q}}^r)\in {\mathcal{X}}^r$

   • $q^r\subset SE(2)$ 是机器人在平面上的配置（位姿）：包括位置 $(x,y)$ 和朝向 $\theta$。
   • ${\dot{q}}^r$ 是机器人在该配置下的速度向量，通常包含线速度和角速度。

2. 可移动障碍物状态 $x^i=(q^i,{\dot{q}}^i,{\ddot{q}}^i)\in {\mathcal{X}}^i$ 对每一个可推拉、可交互的障碍物 $i$（共有 $|\mathcal{O}|$ 个）：

   • $q^i\subset SE(2)$ 同样是物体的位姿；
   • ${\dot{q}}^i$ 是物体当前的速度；
   • ${\ddot{q}}^i$ 是物体当前的加速度（因为我们可能需要在规划时考虑动力学特性）。

3. 整个系统的状态空间 $${\mathcal{X}}^E={\mathcal{X}}^r\times {\mathcal{X}}^1\times \cdots \times {\mathcal{X}}^{|\mathcal{O}|}.$$ 这里的“$\times$”
   就是笛卡尔积，意思是：

   任意一个系统整体状态 ${\mathcal{X}}^E$ 都可以看作是“机器人状态”与“所有可移动障碍物状态”各自取一个值，组合而成的一个元组。

---

二、为什么要计算“笛卡尔积”？

1. 独立自由度的组合 每个子系统（机器人、障碍物1、障碍物2……）都有自己独立的状态空间。机器人可能在某个位置与朝向，障碍物1可能在另一个位置并以特定速度运动……它们之间在数学上是“互不干扰”的独立分量。

   • 笛卡尔积正是把这些独立变量打包在一起：如果你有 A 空间和 B 空间，A×B 就是把 A 中每个可能性都和 B 中每个可能性配对，得到所有“可能的组合”。

2. 完整描述全局场景 在多体系统中，仅知道机器人自己的状态是不够的，也需同时知道所有可动障碍物的状态，才能预测碰撞、规划路径、协调运动。笛卡尔积提供了一种一一对应的方式，让我们在一个大空间里，就能同时对每个体（robot、object₁、object₂…）的状态进行规划和求解。

---

三、为什么整个系统的状态空间能用“笛卡尔积”表示？

1. 数学上的“产品空间”概念

   • 在拓扑学或流形理论里，若 $X$ 和 $Y$ 分别是两个状态空间（各自带有维度、度量、可微结构等），那么它们的“产品空间” $X\times Y$ 也是一个合法的状态空间，其维度是两个空间维度之和，结构上就是把两边“粘”在一起。
   • 机器人有 $n_r$ 维状态（位姿＋速度），每个障碍物又有 $n_i$ 维状态，把它们连起来总就是 $\sum n$ 维。

2. 物理系统可分解性

   • 在很多工程系统中，如果子系统之间不发生内部约束（比如机器人和障碍物不是通过铰链直接连接，而是通过碰撞、接触等外部作用力交互），那么它们的状态就可以当作完全独立的变量来处理。

   • 笛卡尔积正是对“所有独立状态变量的集合”进行数学刻画：只要你给定每个子系统的状态，就唯一地确定了整个系统的状态。

3. 有利于算法实现

   • 实际编程中，路径规划（如 RRT、A*）或最优控制（如 MPC）都需要在整个系统的可行状态空间上搜索。把状态空间写作笛卡尔积后，能很方便地在代码里“层层嵌套”地维护、更新每一个子系统的状态并检查整体约束（碰撞检测、动力学约束等）。

机器人的控制空间记为 $U$，由在时刻 $t$ 以固定持续时间 $\Delta t$ 施加的线速度和角速度 $u_t\in U$ 构成。我们还定义一条长度为 $n$ 步的控制轨迹
$$\tau =⟨u_0,u_1,\dots ,u_{n-1}⟩.$$
我们用 ${\tau }_{[0,n-1]}$ 表示控制序列的子序列，其中 $[0,n-1]$ 是表示起始和结束索引的闭区间。例如，对于 $\tau =⟨u_0,u_1,u_2,u_3⟩$，${\tau }_{[0]}$ 指轨迹的第一个元素 $u_0$，而 ${\tau }_{[1,3]}$ 则指子序列 $⟨u_1,u_2,u_3⟩$。

时刻 $t$ 的环境状态记为
x t = { x r , x 1 , … , x ∣ O ∣ } ∈ X E   . x_t=\{x^r,x^1,\dots,x^{|\mathcal{O}|}\}\in \mathcal{X}^E\,. xt​={xr,x1,…,x∣O∣}∈XE.
系统的离散时间动力学由
$$x_{t+1}=f(x_t,u_t)+\zeta$$
给出，其中 $\zeta$ 表示系统随机性。我们不显式表示 $\zeta$，而是采用在线重规划方法，在执行过程中每一步都使用确定性模型 $f$ 重规划一条新轨迹。

我们称从初始状态 $x_0\in {\mathcal{X}}^E$ 和控制轨迹 $\tau$ 出发，使用展开函数 $R(x_0,\tau )$ 通过模型 $f$ 展开（rollout）轨迹，得到状态序列 $S=⟨x_0,\dots ,x_n⟩$ 为一次“轨迹展开”。我们还定义代价函数 $\mathcal{C}(\mathcal{S})$ 来计算该状态序列的代价。

给定初始状态 $x_0\in {\mathcal{X}}^E$ 和目标物体 $o_g$，目标是让机器人执行一系列控制，将系统从 $x_0$ 移动到抓取到 $o_g$ 的状态，同时应对真实物理不确定性。

IV. 在线重规划与人机交互

我们使用一种基于优化的方法，集成人类输入来解决穿越杂物的到达问题。我们的系统首先完全自主地处理这些问题，并仅在需要时决定请求人类帮助。这样，我们的系统能够在可能的情况下完全自主地解决简单问题，无需任何人工干预。我们的系统在一个统一的在线重规划框架中集成了优化与执行，从而在现实世界中稳健地持续优化并执行解决方案。

在 IV-A 节中，我们描述了所提出的框架 OR-HITL。在 IV-B 节中，我们描述了一种支持 OR-HITL 框架中优化部分的随机优化器。在 IV-C 节中，我们定义了用户输入以及我们如何捕获此输入。在 IV-D 和 IV-E 节中，我们定义了代价函数以及如何计算在优化中使用的初始轨迹。最后，在 IV-F 和 IV-G 节中，我们描述了两种决定何时请求人类帮助的方法。

A. 框架概述

算法 1 描述了 OR-HITL，该算法在一个框架中将优化和执行统一起来，并在两者之间交替，以提高现实世界执行的鲁棒性。

算法以到达目标物体的初始轨迹 $\tau$ （参见图 3a）和具有优化目标的代价函数 $\mathcal{C}$ （见 IV-D 节）开始。在第 2 行，我们观测现实世界状态，如果该状态是目标状态，则停止并声明成功（第 3 行）。否则，继续执行框架的优化部分。

图 3. 初始轨迹。箭头示意机器人轨迹。在 (b) 中，要被推动的物体是箭头穿过的那个箱子。

优化：
在第 5 行，使用 SOLVE 函数（IV-B 节），我们将初始状态 $x^{world}$、当前轨迹 $\tau$ 和当前代价函数 $\mathcal{C}$ 传递给求解器。求解器将在一定时间内进行优化，然后返回结果。结果可以是“需要人类输入”或“成功”。如果求解器返回“成功”，则它还会使用新的轨迹更新 $\tau$。如果求解器决定需要人类输入（我们在 IV-F 和 IV-G 节中描述了该决策的方式），则在第 7 行，我们从人类操作员处获取高层次输入（IV-C 节）。该高层次输入包括用于更新代价函数的信息（第 8 行）和用于实例化新的初始轨迹 $\tau$ 的信息（第 9 行）。我们重复这些步骤，直到结果为“成功”（第 10 行）。一旦优化成功，我们便进行框架的执行部分。

执行：
为应对在现实世界执行轨迹时的物理不确定性，我们提出了一种在线重规划方法。在第 11 行，我们在现实世界执行该轨迹的第一个控制。然后在第 12 行，我们将轨迹 $\tau$ 更新为剩余的 $\tau$ 轨迹，并在末尾添加一个朝向目标的控制 $u_{togoal}$。该在轨迹末尾填充的控制为轨迹在未来进一步优化提供了自由度。更新轨迹后，我们在第 13 行递归，并在第 2 行获取现实世界状态，该状态可能与仿真器预测的不同（物理不确定性）。第 5 行的优化使用上一次迭代的轨迹进行热启动，因此在当前迭代中更易成功，并且几乎不需要求解器额外工作。这种闭环执行以及在优化与执行之间的交替使得我们的系统能够应对执行不确定性，并及早修正轨迹。

B. 随机优化

在算法 1 的第 5 行，我们调用了一个求解器。算法 2 描述了该求解器。该求解器接受一个初始状态 $x_0$、一个待优化的初始轨迹 $\tau$，以及用于计算轨迹代价的代价函数 $\mathcal{C}$。

我们首先在第 2 行从初始状态 $x_0$ 展开轨迹 $\tau$，得到状态序列$\mathcal{S}$，然后使用给定的代价函数 $\mathcal{C}$ 计算该轨迹的代价（第 3 行）。如果求解器成功（第 4 行），则直接返回“成功”（第 11 行）。要判断求解器是否成功，我们检查机器人是否已到达目标并且代价是否降至某个阈值以下。如果求解器未能成功，我们继续进行优化。

首先在第 5 行检查是否需要人类帮助。我们在 IV-F 和 IV-G 节中描述了如何做出该决策。如果需要人类帮助，则返回“需要人类帮助”的信号（第 6 行）。

如果不需要人类帮助，则在第 7 行从 $\tau$ 中采样 $k$ 条带噪声的轨迹。为了生成这 $k$ 条轨迹，我们对 $\tau$ 的控制量添加方差为 $v$ 的高斯噪声 $\mathcal{N}(0,v)$，其中 $\mathcal{N}$ 表示高斯分布，$v$ 是机器人某一自由度的方差。然后在第 8 行，我们使用展开函数 $R$ 从 $x_0$ 展开每条噪声轨迹，并在第 9 行计算它们各自的代价。在第 10 行，将 $\tau$ 更新为代价最低的那条轨迹。如果所有 $k$ 条轨迹的代价都高于当前 $\tau$，则 $\tau$ 保持不变。我们重复这些步骤，直到求解器成功（第 4 行）。

C. 用户输入

用户的高层次操作建议将特定物体 $o_i$ 推动到平面上的特定点。我们用三元组 $(o_i,x_i,y_i)$ 来形式化这一高层次操作，其中 $o_i\in \mathcal{O}$ 是一个物体，$(x_i,y_i)$ 是需将 $o_i$ 推至的平面上的点。

为了捕获用户的高层次操作，我们开发了一个简易用户界面。界面向操作员展示环境和机器人。操作员通过鼠标指针，先点击所需物体，然后点击平面上的目标点来提供输入（算法 1 第 7 行）。

利用该人类输入，我们即可在下一小节中定义代价函数和利用人类输入的初始轨迹。

D. 代价函数

代价函数 $\mathcal{C}$ 在算法 2（第 3 行和第 9 行）中使用，同时也在算法 1（第 8 行）中更新以集成人类输入。

未提供人类输入：
如果没有提供人类输入，则状态序列 $S$ 的代价函数定义为 $\mathcal{C}(\mathcal{S})={\mathcal{C}}_1+{\mathcal{C}}_2$。其中，${\mathcal{C}}_1$ 是到达目标物体的代价：$${\mathcal{C}}_1=w_g\cdot \text{d}({\text{q}}^{\text{ee}},{\text{q}}^{\text{g}})(1)$$
${\mathcal{C}}_1$ 表示机器人末端执行器到目标物体 $o_g$ 的加权欧氏距离。${\mathcal{C}}_2$ 定义了三个代价项及其对应权重：
$${\mathcal{C}}_2=\sum _{i=1}^n(c_e(x_i)+c_f(x_i)+c_s(x_i))(2)$$

• $c_f(x_i)={\sum }_{j=1}^{|\mathcal{O}|}{w}_f\cdot F(x_i^j)$：在状态 $x_i$ 下，对任何对其他可动或静态障碍物施加高接触力的可动物体进行惩罚。函数 $F$ 给定物体状态 $x_i^j$，返回该物体的接触力。
• $c_e(x_i)={\sum }_{j=1}^{|\mathcal{O}|}{w}_e\cdot 1_e(q_i^j)$：在状态 $x_i$ 下，对任何几何上超出配置空间的可动物体 $o_j\in \mathcal{O}$ 进行惩罚。指示函数 $1_e$ 若物体在配置空间内返回 0，否则返回 1。
• $c_s(x_i)=w_s\cdot 1_s(x^r)$：在状态 $x_i$ 下，对机器人与任何静态障碍物发生碰撞进行惩罚。指示函数 $1_s$ 若机器人 $r$ 与任何静态障碍物碰撞返回 1，否则返回 0。

提供人类输入：
如果提供了人类输入（算法 1，第 7 行），则代价函数将进行双重更新（算法 1，第 8 行）。首先使用 $\mathcal{C}(\mathcal{S})={\mathcal{C}}_3+{\mathcal{C}}_2$ 将被人类指示的物体推至指定位置，然后使用 $\mathcal{C}(\mathcal{S})={\mathcal{C}}_1+{\mathcal{C}}_2$ 来到达目标物体。其中
$${\mathcal{C}}_3=w_p\cdot \text{d}({\text{q}}_{\text{n}}^{\text{i}},{\text{q}}_{\text{desired}}^{\text{i}})$$
对于高层输入 $(o_i,x_i,y_i)$，${\mathcal{C}}_3$ 表示被推物体 $o_i$ 在最终状态 $q_n^i$ 与人类提供位置 $q_{desired}^i$ 的加权欧氏距离。此代价项将鼓励优化器探索将人类指示物体推向期望位置的解。

E. 初始轨迹

在开始优化时，我们需要为求解器提供一个初始轨迹。我们使用直线轨迹，因为它们计算成本低（无需物理仿真）。

图 3. 初始轨迹。箭头示意机器人轨迹。在 (b) 中，要被推动的物体是箭头穿过的那个箱子。

我们在图 3 中示意了两种此类初始轨迹。第一条轨迹（图 3a）是到达目标物体的初始轨迹，它是一条从机器人的当前位置 $q^r$ 到目标物体位置 $q^g$ 的直线轨迹。第二条轨迹（图 3b）是将物体推至期望位置的初始轨迹，该轨迹沿着从物体当前位置出发并以物体期望位置为终点的直线。

接下来，在 IV-F 和 IV-G 节中，我们描述了两种决定何时请求人类输入的方法。此决策在算法 2 的第 5 行作出，我们在后文详细阐述这两种不同的决策方式。

F. 使用固定超时请求人类帮助

一种直接决定是否需要人类输入的方法是基于固定超时限制。求解器在某个固定时间限制内尝试寻找解，如果未能在该时间内找到解，就会超时并请求人类输入。

我们将该时间限制记为 Fixed $z$，其中 $z\in {\mathbb{Z}}^{+}$。因此，在算法 2 的第 5 行中，当使用 Fixed OR-HITL 时，如果达到 $z$ 秒，humanHelpRequired 就会返回 true。例如，对于 Fixed 20，如果求解器在 20 秒内无法找到解，humanHelpRequired 就会每 20 秒返回一次 true。

尽管这是一种简单直接的方法，但在某些情况下可能存在问题。例如，如果求解器能够在 25 秒内解决问题，但固定时间限制设为 20 秒，那么求解器会在 20 秒时请求人类帮助，尽管如果再多给 5 秒，它本可以找到解。同样地，对于一个很难的问题，延长超时限制可能会导致系统在请求人类帮助之前不必要地浪费时间。固定超时方法的这一缺点促使我们引入一种自适应方法，根据当前问题的难度来决定何时请求人类帮助。

G. 自适应请求人类帮助

轨迹优化的一个特性是：从一次迭代到下一次迭代的代价收敛速率，可以表明求解器是在探索新解（即需要更多时间），还是已经陷入了局部极小值（即此时立即请求人类输入可能更有帮助）。我们利用这一特性，根据当前问题自适应地决定何时请求人类帮助。如果在优化过程中某一点发现求解器陷入了局部极小值，就发出需要人类输入的信号。

为了判断求解器是否陷入局部极小值，我们观察上一次迭代的代价 $c_{previous}$ 与当前迭代的代价 $c_{current}$ 之间的绝对差 $|c_{\text{current}}-c_{\text{previous}}|$。如果该差值在若干连续迭代中都低于某个阈值，就认为已经陷入局部极小值。由于这是随机优化，单次迭代中代价未必改善并不一定意味着陷入局部极小值，因此需要连续多次迭代进行检查。

因此，在算法 2 的第 5 行，当使用 Adaptive OR-HITL 时，如果在连续若干次迭代中检测到局部极小值，humanHelpRequired 就会返回 true。在我们的实现中，当连续 2 次迭代都满足局部极小值条件时，就停止并请求人类帮助。

V. 实验与结果

我们在评估中聚焦三大问题。首先，在 V-B 节中，我们评估了所提框架在人机交互下的性能，并与多个基线方法进行了比较。其次，在 V-C 节中，我们评估了在物理不确定性存在情况下，框架的在线重规划能力。第三，在 V-D 节中，我们在单人操作员的模拟仓库场景中评估了我们的系统。这些实验中的部分内容在附带的视频¹中有展示。我们在这些实验中使用了两名操作员：

• 一位经验丰富的用户（也是本文作者之一），负责了本节中所有人类参与的实验，
• 除了标注为“新手”的实验部分，由一名对机器人系统或机器人研究毫无经验的新手操作员完成。

附带视频中展示了部分实验过程与结果（https://youtu.be/t3yrx-J8IRw）。

A. 实验设置

在所有实验中，我们使用 MuJoCo2 [41] 实现系统动力学。我们采用随机生成器来创建随机仿真场景。该生成器首先将目标物体放置在货架后方，然后在目标物体周围半径 $30cm$ 范围内的无碰撞位置随机放置其余物体。每次实验的总时间限制为 $180s$，超时后机器人停止运行，该次实验视为失败。此时间限制包括优化时间和人机交互时间。

优化参数：
优化参数设为：每次迭代采样 $k=15$ 条噪声轨迹，机器人线速度方差为 $0.04m/s$，角速度方差为 $0.04rad/s$。我们使用时长 $3s$、包含 $8$ 步的轨迹。以积分步长 $0.0015$ 展开一条 $3s$ 轨迹时，平均耗时约 $1s$；在仿真中执行该轨迹约需 $3s$。代价函数参数为：$w_g=2000$、$w_f=50$、$w_e=300$ 和 $w_s=300$。最后，成功阈值设为 $70.0$（算法 2，第 4 行）。

机器人：
我们在一台安装于 Clearpath Ridgeback 底盘上的 UR5 机器人上使用 Robotiq 两指夹爪。在仿真中，我们控制手部 3 个自由度：2 个线速度和 1 个角速度。夹爪本身有 1 个自由度，但我们不将其纳入优化，而是在优化开始时关闭夹爪，并在轨迹最后一步之前打开夹爪。在现实世界中执行解时，我们将夹爪速度映射为 Ridgeback 的车体速度。

B. 框架评估

我们比较了 Fixed 5、Fixed 20 和 Adaptive 三种方式，在一位经验丰富的用户和一位新手用户下，并与两种全自主规划器进行了对比。新手用户对系统或问题毫无先验知识，我们对其进行了大约45分钟的培训。

我们为每个规划器生成了30个随机场景。为了避免用户记住问题模式，我们针对每个人机结合的规划器都生成了一套不同的问题集。对于 Autonomous 和 Heuristic 规划器，由于不存在学习过程，我们在所有场景上都进行了评估。

这里我们比较了两种全自主规划器。第一种 “Autonomous” 规划器使用了算法 2 中的求解器，但目标仅仅是直接到达目标物体，而不考虑任何高层次规划。我们还实现了一种直线启发式方法来替代 OR-HITL 框架中的人类输入：机器人沿直线路径移动到目标物体处，识别第一个阻挡障碍，并计算机器人的扫掠体积；然后在该扫掠体积外为该障碍物寻找一个无碰撞的位置；最后将该物体和新位置返回给框架（即完全替代人工输入），并规划推动该物体到新位置。我们在图 4 中展示了这种直线启发式的过程，并将该规划器称为 “Adaptive Heuristic”。

图 4. 启发式：黄色区域为机器人扫过的范围，白色箭头为建议的高级计划。

表 I 给出了仿真结果。规划时间是每个问题的平均规划时长；人类时间是用户提供指导的平均耗时；总时间则包括规划时间、人类时间（如适用）以及执行时间，反映了解决一个问题的整体平均时长。结果表明，带有人机交互的规划器在成功率上优于全自主规划器，且规划所需时间大幅减少。Adaptive 方法平均每个问题仅需 2.5 秒的人类干预时间。Fixed 5 方法则更频繁地请求人类帮助，表明该策略下人机交互的强度相当高。新手用户也确认，与 Fixed 5 的交互更加繁琐，因为提示过于频繁，他觉得自己很难在 5 秒超时限制内提供有意义的高层次建议。相较之下，新手参与者认为 Adaptive 方法更有趣、更易使用，并且他很喜欢机器人能够自主解决更多问题的表现。

C. 应对不确定性

我们的框架在执行过程中旨在应对物理不确定性。为了评估这一特性，我们进行了两种不同的实验。首先，在仿真中评估系统，在执行期间向物体运动添加人工不确定性。其次，在真实机器人于真实货架上操作物体的实验中评估系统。

仿真：
我们按如下方式配置仿真环境：在每个仿真步（仅在执行期间，而非优化期间），观测所有可移动物体的速度，若其超过某个阈值（为了避免向自上一步以来未移动的物体添加不确定性），则向它们的速度添加小幅高斯噪声（改变方向和大小）。我们对比了 Adaptive、Autonomous 以及作为基线的开环自主（OL Autonomous）规划器。OL Autonomous 仅在轨迹执行结束时重规划，而非在每个动作结束时。表 II 给出了结果。执行失败次数表示规划器找到初始解但在执行和重规划时失败的次数。重规划迭代次数表示为成功重规划一次执行失败的轨迹所需的优化迭代次数。总时间是完成任务的平均时间（规划时间、人类时间和执行时间）。结果表明，Adaptive 在成功率和规划时间上均优于 Autonomous 和 OL Autonomous。Adaptive 的成功率为 97%，Autonomous 为 58%，OL Autonomous 为 38%。在重规划过程中，Adaptive 和 Autonomous 平均分别需要 3 次和 4 次迭代来纠正轨迹，而 OL Autonomous 则平均需要 12 次迭代。

真实世界：
在真实环境中，我们评估了 Adaptive 和开环 Adaptive（OL Adaptive）。OL Adaptive 在轨迹结束时重规划，而非在每个动作结束时。我们进行了 30 次真实世界实验，每个规划器在 15 个不同场景中各测试 15 次。机器人被要求在一个小货架中取绿色物体，该货架上还放置了 9 个其他障碍物。图 5 展示了其中一些场景。为了避免物理机器人或环境中物体的损坏，如果机器人与任何静态障碍物发生碰撞或用力将物体推到货架上，我们就停止机器人并将该尝试视为因违反安全规则的执行失败。在附带视频中也演示了这些实验（由于每次动作执行后调用感知系统的延迟，“Adaptive” 执行比 “OL Adaptive” 稍慢）。表 III 给出了结果。Adaptive 的成功率为 86%，而 OL Adaptive 为 53%。Adaptive 在规划过程中失败一次，并在执行过程中因违反安全规则失败一次。OL Adaptive 在规划过程中失败一次，在执行过程中失败六次。OL Adaptive 失败的主要原因是物理不确定性导致机器人违反部分安全规则。由于 OL Adaptive 不在每一步重规划，通常在执行轨迹时更快，但一旦失败则需要更多重规划时间。相比之下，Adaptive 会对某些简单问题实例进行重规划，但由于我们对优化进行了热启动，重规划时间始终很短。

图 5. 真实世界的场景。

D. 仓库问题

我们考虑一个场景：在模拟仓库的杂乱货架上，需要拣取50个订单的物体。仓库同时运行六台机器人。我们的目标是提高这些机器人的成功率，并尽可能快速地完成这50个订单。

在仿真中，我们比较了六台并行工作的全自主机器人（Autonomous）尝试完成这50个订单，与由单个人类操作员同时指导六台机器人的系统（Adaptive）的表现。图1展示了其中的一些场景。每台机器人都尝试在180秒内解决分配的问题。一旦一台机器人完成当前任务，它就会被分配下一个可用任务。由于同时运行六个仿真实例对CPU和内存要求极高，我们在一台更强大的计算机上进行了实验。

结果（表IV）表明，我们的方法更为成功（成功率74%，而Autonomous仅为32%），并且在每个问题的规划时间上也更快，平均快了近1分钟。Adaptive方法的平均动作次数为4次，因此每个问题的执行时间（每条轨迹固定为3秒）平均增加了12秒。相比之下，Autonomous直接规划到目标，仅有一条轨迹，因此每个问题仅有3秒的执行时间。

VI. 结论

我们提出了OR-HITL，一种基于轨迹优化的在线重规划与人机交互框架。该方法在开始时完全自主地解决问题，并仅在问题被评估为过于困难时才请求人类输入。我们使用自适应方法（Adaptive）根据问题难度作出这一决策。我们在模拟仓库场景中展示了该自适应性在单个操作员管理多台机器人时的有效性。最后，由于框架中实现的在线重规划策略，我们的方法在现实世界执行时表现出更高的鲁棒性。