论文笔记（四十八）Online state vector reduction during model predictive control with gradient-based traject

文章概括

引用：

@article{
    
    russell2024online,
  title={
    
    Online state vector reduction during model predictive control with gradient-based trajectory optimisation},
  author={
    
    Russell, David and Papallas, Rafael and Dogar, Mehmet},
  journal={
    
    arXiv preprint arXiv:2408.11665},
  year={
    
    2024}
}

Russell, D., Papallas, R. and Dogar, M., 2024. Online state vector reduction during model predictive control with gradient-based trajectory optimisation. arXiv preprint arXiv:2408.11665.

原文：https://arxiv.org/abs/2408.11665
代码、数据和视频：https://arxiv.org/abs/2408.11665

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

摘要

在高维系统中，非抓取操作面临着多种挑战，其中一个主要原因是大状态空间导致的计算规划时间过长。轨迹优化算法在多种任务中已证明其有效性，但像大多数方法一样，它们在高维系统，尤其是在杂乱环境中的非抓取操作和变形物体操作中扩展性有限。我们推测，在操作过程中，随着系统的发展，不同的自由度在任务中变得更重要或不那么重要。我们利用这一想法减少轨迹优化问题中考虑的自由度数量，从而缩短规划时间。这个想法在模型预测控制（MPC）的背景下尤为相关，因为优化问题的成本景观在不断变化。我们提供了异步MPC下的仿真结果，表明我们的方法能够实现更好的整体性能，因其减少了策略延迟，同时仍能有效地优化轨迹。

关键词： 非抓取操作 · 模型预测控制 · 轨迹优化 · 维度减小

1. 介绍

轨迹优化算法能够合成复杂的运动，以解决各种具有挑战性的机器人操作任务。然而，轨迹优化方法在高维系统（如在杂乱环境中的操作或对变形物体的操作）上扩展性较差。维度诅咒导致的长优化时间，使得在这些高维系统中执行闭环模型预测控制变得困难。

在本文中，我们通过动态降低这些系统的维度来解决这一问题，以实现闭环模型预测控制（MPC）。我们的关键见解是，在各种高维任务中，大量的自由度（DoFs）在任务执行的所有时刻并不总是与问题相关。这种直觉在基本层面上是合理的：人类在执行各种任务时并不会考虑系统中所有自由度的完整物理效应。当我们叠衣服时，并不会详细考虑布料中每个粒子将采取的路径；同样，当我们伸手去取杂乱架子上的物品时，我们善于识别哪些物体对我们的目标重要，而忽略其他物体。

图1展示了我们的方法为杂乱环境中的非抓取操作任务生成的轨迹。传统上，在这种场景的轨迹优化过程中，所有可移动刚性物体的六个自由度（共贡献了十二个状态向量元素，即每个自由度的位置和速度元素）与机器人自由度一起被考虑在系统状态中。相反，我们提出了一种方法，能够在任务的不同时间识别相关的自由度，仅在减少的系统状态下进行轨迹优化。

扫描二维码关注公众号，回复： 17457321 查看本文章

图1：一系列快照展示了我们方法生成的一个MPC轨迹示例。任务是将绿色圆柱体推到目标区域（绿色透明圆柱形区域），同时尽量减少对一些杂物的干扰。该系统的自由度总数为55。我们的方法在执行过程中识别该系统在不同时间的相关自由度，并使用该减少的状态进行轨迹优化。深红色阴影的物体在执行过程中具有更多的自由度：如果一个物体呈深红色，则其所有六个自由度都被考虑；如果一个物体呈白色，则在轨迹优化过程中没有考虑其六个自由度。

在机器人领域，曾经使用降维模型进行高效的规划和控制。运动是一个这样的领域，其中降维模型在实现高维机器人的实时闭环控制方面表现出色。这些模型可以是预定义的，例如倒立摆模型，或根据任务描述学习得出的模型。虽然可以对运动采取这样的降维方法（在这种情况下，完整模型几乎总是局限于机器人本身，而降维模型是相同机器人的低维近似），但对物体操作任务（如图1所示）采取类似的方法更为困难，因为完整模型可能包括任意数量、形状和配置的物体，而这些在之前是未知的。这就是为什么在本研究中，我们考虑在任务执行过程中在线降维的方法（MPC），以适应特定的任务实例。此外，减少的状态可以在任务的不同阶段有所不同，如图中的不同快照所示。

重要的是，我们并没有提取系统的简化动力学模型，而仅仅提取了简化的状态向量。我们展示了使用简化状态向量可以显著提高（(以优化时间计算））优化效率（特别是在使用基于梯度的轨迹优化方法时，例如迭代线性二次调节器（iLQR）[14]）。这些方法通常需要计算系统动力学的导数，通常通过计算代价较高的有限差分法来完成[22]，而状态向量的缩小可以大幅降低计算成本。轨迹优化中的其他操作，如价值函数的反向传播，也从状态向量的缩小中受益。因此，在本文中，我们还介绍了对iLQR方法的修改，使得我们可以使用简化状态向量进行优化，同时在模拟器中前向/动力学模型仍然使用完整状态[27]。

我们提出并比较了不同的方法来识别系统的相关自由度（DoFs）。首先，我们提出了一种简单的方法，该方法认为一个自由度是相关的，当且仅当它出现在给定任务的代价函数中。尽管这种方法在一般情况下是一个不错的启发式方法，但它可能会遗漏一些重要的自由度，这些自由度并没有出现在代价函数中（例如，一个不在代价中考虑的物体推动了另一个在代价中考虑的物体），同时也可能包含一些不总是相关的自由度（例如，一个在代价函数中考虑的物体，但在当前状态和标称轨迹下并不影响代价）。因此，我们还研究了几种方法，试图识别当前状态和标称轨迹附近的相关自由度。为此，我们利用了LQR增益矩阵K，该矩阵表明了为了达到最优行为，当前状态的变化应如何导致当前控制的变化。我们提出了两种基于K的方法：第一种方法直接使用K矩阵列中的值来识别那些能够引起控制显著变化的状态元素，因此这些状态元素被认为是相关的。第二种方法对K进行SVD分解，识别出主轴，并利用它们来找出最重要的状态元素。我们将这三种方法与两个基线方法进行比较：一种是始终使用完整状态向量的原始iLQR，另一种是优化过程中随机选择状态向量的子集。

我们的结果表明，基于K的方法可以在任务执行过程中的不同点识别出任务的相关自由度（这一点在图1中也有所体现），从而显著降低优化时间。在MPC循环中，优化时间的减少意味着策略延迟的降低，因此相比于基线方法，MPC性能得到了提升。

本文做出了以下贡献：

1. 我们提出了一种通用的MPC方法，该方法可以在线改变状态向量中的元素以最小化优化时间，进而减少策略延迟。
2. 我们提出并比较了不同的方法，用于识别给定任务下状态向量中相关的自由度。
3. 我们对iLQR进行了修改，以实现简化状态向量的轨迹优化。
4. 我们在三个高维非抓取操作任务上评估了我们的方法。

2. 相关工作

轨迹优化在机器人领域传统上用于寻找平滑、无碰撞的运动计划 [11,21]。近年来，其应用已扩展到更复杂的操作和运动任务，例如手内操作与抓取 [4,6]、全身操作 [13]、运动 [20] 以及在杂乱环境中的物体操作 [12,1,17]。

在杂乱环境中的操作面临的一个挑战是物理不确定性，即在仿真中优化的轨迹往往在现实世界中因不可预测的物理交互而失败。为了解决这个问题，通常采用模型预测控制（MPC） [8,31,19]。MPC 使用实时模型预测来优化控制动作，使系统能够适应发生的变化和不确定性。然而，轨迹优化在高维空间中表现不佳，主要是由于维度的诅咒，这会导致较长的优化时间。随着任务复杂度的增加，维度数目也随之增加，使得高效的优化和 MPC 变得越来越困难。

各种研究探索了加速运动规划的方法 [2,23,17]。例如，Saleem 和 Likhachev [23] 开发了一种规划方法，该方法仅在必要时使用物理模拟器，而在远离障碍物的动作中依赖于更便宜的几何模型，以减少计算时间。尽管这些方法通常能够在几秒钟内优化出解决方案，但这个时间长度对于实时 MPC 来说仍然具有挑战性。

在机器人领域中，已有多种降维方法被探索。例如，快速探索随机树（RRT）在搜索空间维度增加时难以找到有效的运动计划。Zheng 等人 [32] 通过减少采样状态空间的维度，来减少找到最优解所需的时间。类似地，Vernaza 等人 [28,29] 针对抽象机器人的运动问题进行了研究，这类机器人具有大量自由度（DoFs），需要在杂乱的空间中导航以到达某些期望配置，同时避免碰撞。另一种形式的降维由 Hogan 和 Rodriguez [7] 提出，他们为一个二维推滑系统提出了直接转录轨迹优化问题，并考虑了在长轨迹时间线上接触模式切换的指数特性。为了限制这种指数爆炸，他们限制了接触模式在轨迹中切换的次数，从而使问题在计算上可行。

操作可变形物体本质上是一个高维问题，通常会导致较长的运动规划时间。Wang 等人 [30] 针对将一维和二维软体（如绳索和布料）操作至目标配置的问题进行了研究。为了减少计算成本，他们计算了一组最小的关键点，这些关键点由其算法操作，从而实现折叠任务的高效规划。Mahoney 等人 [15] 则通过主成分分析，研究了减少软体机器人维度，以便在狭小环境中计算无碰撞的运动计划。

值得注意的是，Sharma 和 Chakravorty [25] 提出了类似于我们的方法的降阶 iLQR 实现，其中的滚动更新是使用完整系统动力学进行的。他们对给定的高维偏微分方程（PDEs）进行优化直到收敛。我们的工作重点是机器人操作问题，其中偏微分方程的解析形式并不容易获得。我们还专注于在线执行期间动态调整状态向量的大小。

Ciorcarlie 等人 [6] 通过将抓取次数限制在一组关键的“特征抓取”（eigengrasps）中，探索了减少抓取问题维度的方法。他们展示了这一简化的动作空间仍然能够抓取多种复杂的物体。Jin 等人 [9] 研究了复杂三指操作的降阶混合模型的学习，实现了实时闭环 MPC。Chen 等人 [5] 针对双足机器人运动问题，提出了学习降阶模型的方法。在他们的工作中，他们制定了一系列双足机器人执行的任务（如在平地行走、上坡行走等），希望离线计算一个可用于所有指定任务的降阶模型。他们手动指定了一个特征向量（从全阶模型中提取的低维特征组合），并通过使用当前的降阶模型进行轨迹优化，离线学习一组线性权重。线性权重通过随机梯度下降进行调整。

这些学习方法利用任务的先验分布来学习降阶模型。然而，在机器人对象操作中，物体的数量和类型通常无法提前知道，每个问题可能需要不同的降阶模型。我们提出了一种方法，能够在 MPC 执行期间动态调整系统状态。在任意给定的时间点，我们利用轨迹优化器的信息，识别当前任务配置中最相关的自由度，并在这个简化的状态空间中进行优化。我们的方法不假设任何任务的先验分布，并评估了其在杂乱环境中操作刚性和可变形物体的有效性。

3. 问题定义

我们考虑的是离散时间动力学公式，

$${\text{x}}_{t+1}=f({\text{x}}_t,{\text{u}}_t)\text{\hspace{1em}(1)}$$

其中，${\text{x}}_t$和 ${\text{u}}_t$ 分别是轨迹上某一时间步 $t$ 的状态向量和控制向量。

我们有一些运行成本函数；

$$l({\text{x}}_t,{\text{u}}_t)=({\text{x}}_t-{\tilde{\text{x}}}_t{)}^{\top }W({\text{x}}_t-{\tilde{\text{x}}}_t)+{\text{u}}_t^{\top }R{\text{u}}_t\text{\hspace{1em}(2)}$$

(这个公式表示的是一个带权重矩阵$W$ 和 $R$ 的二次型成本函数，应用于状态向量 ${\text{x}}_t$ 和控制向量 ${\text{u}}_t$。)

其中$W$和$R$是半正定的成本加权矩阵，${\tilde{\text{x}}}_t$是时间步$t$时的期望状态。

轨迹的总运行成本$J$是所有运行成本的总和，以及从初始状态${\text{x}}_0$施加控制序列$\text{U}\equiv ({\text{u}}_0,{\text{u}}_1,\cdots ,{\text{u}}_{T-1})$时的终端成本函数。

$$J({\text{x}}_0,\text{U})=l_f({\text{x}}_T)+\sum _{t=0}^{T-1}l({\text{x}}_t,{\text{u}}_t)\text{\hspace{1em}(3)}$$

(这个公式表示的是总成本$J(x_0,\text{U})$，由终端成本$l_f({\text{x}}_T)$和从$t=0$到$T-1$的运行成本的总和组成。)

其中，$l_f$是终端成本函数，形式与公式 (2) 相同，但使用不同的矩阵$W_f$和$R_f$。我们还将通过施加控制序列$\text{U}$得到的完整轨迹状态序列表示为$\text{X}\equiv ({\text{x}}_0,{\text{x}}_1,\cdots ,{\text{x}}_T)$。

一般的轨迹优化问题是计算一组最优控制序列，使得从某个初始状态开始的轨迹的总运行成本最小化。

$${\text{U}}^{\ast }({\text{x}}_0)=\arg \underset{\text{U}}{\min }J({\text{x}}_0,\text{U})\text{\hspace{1em}(4)}$$

(这个公式表示从初始状态${\text{x}}_0$开始，使总成本$J({\text{x}}_0,\text{U})$最小化的最优控制序列${\text{U}}^{\ast }({\text{x}}_0)$。)

我们感兴趣的是在模型预测控制 (MPC) 框架中使用轨迹优化。在每次MPC迭代期间，都会求解公式 (4) 中的优化问题，优化后的控制量会在实际（在这项工作中，我们使用模拟器来代表真实系统。）系统上执行，同时以当前系统状态为基础，启动新一轮的优化。因此，控制频率由公式 (4) 的求解速度决定，这对MPC方案的性能有显著影响。

设${\underline{\text{x}}}_t$表示MPC执行时刻$t$的实际系统状态，并假设MPC执行了$Y$次控制操作后停止（如果达到任务超时或成功条件，我们就会停止控制器。）。我们使用$\text{MPC\_Cost}$来量化MPC的性能：

$$\text{MPC\_Cost}=l_f({\underline{\text{x}}}_Y)+\sum _{t=0}^{Y-1}l({\underline{\text{x}}}_t,{\text{u}}_t)\text{\hspace{1em}(5)}$$

上面的表达式类似于公式 (3)，并使用了相同的成本函数 $l$ 和 $l_f$。不同之处在于，公式 (3) 是基于动力学模型预测的状态 ${\text{x}}_t$ 进行计算，而公式 (5) 则是基于系统实际达到的真实状态进行计算。此外，公式 (3) 是对优化时间范围 $T$ 进行求和，而公式 (5) 则是对整个执行过程的时长 $Y$ 进行求和。

在本工作中，我们的目标是通过更快地求解公式 (4) 来改进 $\text{MPC\_Cost}$，从而在MPC期间实现更高的控制频率。

3.1 定义

我们考虑的操作任务由具有 $q$ 个关节的机器人和多个物体组成。系统中有 $N_O$ 个刚性物体和 $N_S$ 个可变形/软体物体。刚性物体简单地用六个自由度表示它们的姿态。可变形物体 $i$ 由 $N_{Si}$ 个粒子组成，每个粒子有三个自由度 { x , y , z } \{x, y, z\} { x,y,z}。我们用 $\mathcal{F}$ 表示系统中所有自由度的集合。所以：

$$|\mathcal{F}|=q+6N_O+3\sum _{i=1}^{N_S}{N}_{S_i}\text{\hspace{1em}(6)}$$

在本工作中，我们感兴趣的是发现并使用一个自由度的缩减集合 $\mathcal{C}\subseteq \mathcal{F}$。当涉及系统向量或矩阵的缩减版本时，我们将使用上标表示。例如，${\text{x}}_t^{\mathcal{C}}$ 表示缩减状态向量，只包括对应于集合 $\mathcal{C}$ 中自由度的元素。如果使用所有自由度集合 $\mathcal{F}$，则不使用上标。

我们考虑状态向量中的每个自由度的位置信息和速度信息。因此，${\text{x}}_t\in {\mathbb{R}}^{2|\mathcal{F}|}$，而 ${\text{x}}_t^{\mathcal{C}}\in {\mathbb{R}}^{2|\mathcal{C}|}$。控制向量的大小为 $m$，${\text{u}}_t\in {\mathbb{R}}^m$，大小不变。

我们用 $\mathcal{L}$ 表示当前未包含在缩减自由度集合 $\mathcal{C}$ 中的自由度，即 $\mathcal{L}=\mathcal{F}\setminus \mathcal{C}$。

4 方法

一个通用的异步MPC方案（类似于Howell等人[8]）如算法1所示。智能体连续执行优化后的控制序列$\text{U}$。优化器持续从智能体查询当前状态（第5行）并优化控制序列$\text{U}$（第9行）。根据优化器优化控制序列$\text{U}$（在 MPC 中，我们只进行一次优化迭代，而不是优化到结束。）所需的时间，智能体可能已经超越了优化器初始化时的状态。因此，优化时间越长，控制序列就越不适用；这通常被称为策略延迟。

在本工作中，我们通过动态改变第9行中使用的状态向量的大小来扩展这一通用MPC方案，以减少优化时间，从而降低策略延迟。我们在第1行初始化我们减少的自由度集$\mathcal{C}$：在本工作中，我们简单地设置$\mathcal{C}=\mathcal{F}$，但如果有更好的启发式方法可以代替。在每次优化调用之前，我们都会在 $\mathcal{L}$ 中找出一些未使用的 DoFs，并在第 6 行将其重新引入到缩小后的 DoFs $\mathcal{C}$ 中。然后我们使用减少的DoFs自由度集优化控制序列，在计算出控制序列后，我们在第10行识别出在先前轨迹优化问题中不重要的一组自由度。这些自由度随后会从我们减少的自由度集中移除。

该方法有三个重要组成部分。首先，我们需要一种仅使用减少的DoFs自由度集优化轨迹的方法（算法1，第9行）。这一部分在第4.1节中解释，我们在那里阐述了如何对减少的自由度集执行iLQR [14,26]。其次，我们需要一种方法来确定哪些自由度可以被移除（算法1，第10行）。在第4.2节中，我们概述了不同的方法来识别要移除的自由度。最后，我们需要一种方法将自由度DoFs重新引入减少的DoFs自由度集中（算法1，第6行）。在本工作中，我们提出了一种简单的随机抽样策略，从未使用的自由度集DoFs $\mathcal{L}$ 中选择自由度。如果利用特定任务和当前轨迹的信息，这种自由度重新引入方法可以轻松改进。然而，本工作的目的是提出一种任务无关的在线状态向量减少抽象方法，因此我们没有进一步探讨这一方向。

4.1 优化

在本节中，我们概述了如何扩展iLQR [14,26]算法，使其在减少的自由度集$C$上运行。高层次的概述是，我们仅对减少的自由度集进行导数计算和反向传递计算。我们使用MuJoCo模拟器对整个系统进行建模。当我们执行前向展开时，MuJoCo会更新完整的自由度集，并计算轨迹的总运行成本。我们将这种iLQR适应称为状态向量减少的iLQR（iLQR-SVR）。

iLQR-SVR使用系统动态的一阶近似，其中我们将方程1写为方程7：

$${\text{x}}_{t+1}^{\mathcal{C}}=A_t^{\mathcal{C}}{\text{x}}_t^{\mathcal{C}}+B_t^{\mathcal{C}}{\text{u}}_t\text{\hspace{1em}(7)}$$

其中$A_t^{\mathcal{C}}=\delta f({\text{x}}_t^{\mathcal{C}},{\text{u}}_t)/\delta {\text{x}}_t^{\mathcal{C}}$，$B_t^{\mathcal{C}}=\delta f({\text{x}}_t^{\mathcal{C}},{\text{u}}_t)/\delta {\text{u}}_t$。iLQR还需要对状态和控制向量的一阶和二阶成本导数进行近似计算（$l_{\text{x}}^{\mathcal{C}}$, $l_{\text{xx}}^{\mathcal{C}}$, $l_{\text{u}}$, $l_{\text{uu}}$）。

动态导数的计算 A ≡ { A 0 , A 1 , … , A T − 1 } A ≡ \{A_0, A_1, \dots, A_{T-1}\} A≡{ A0​,A1​,…,AT−1​}和 B ≡ { B 0 , B 1 , … , B T − 1 } B ≡ \{B_0, B_1, \dots, B_{T-1}\} B≡{ B0​,B1​,…,BT−1​}通常是基于梯度的轨迹优化的瓶颈。这些导数通常需要通过计算成本高的有限差分来计算。有限差分要求为系统中的每个自由度以及任何控制输入评估系统动态。通过仅为我们减少的状态集（$A^{\mathcal{C}}$和$B^{\mathcal{C}}$）计算动态导数，我们将动态评估的数量从$2|\mathcal{F}|+m$减少到$2|\mathcal{C}|+m$。

使用这些近似，最优控制修改可以通过动态规划原则递归计算[14,26]。这个步骤通常称为反向传递，其工作原理是通过计算方程 8-10 将值函数 $V$ 从轨迹终点传播到起点，从 $t=T$ 到 $t=0$。我们扩展了以下方程，以便在减少的自由度集上运行：

$$Q_{\text{x}}^{\mathcal{C}}=l_{\text{x}}^{\mathcal{C}}+(A^{\mathcal{C}}{)}^{\top }{V}_{\text{x}}^{{}^{\prime }\mathcal{C}}\text{\hspace{1em}(8a)}$$

$$Q_{\text{u}}=l_{\text{u}}+(B^{\mathcal{C}}{)}^{\top }{V}_{\text{x}}^{{}^{\prime }\mathcal{C}}\text{\hspace{1em}(8b)}$$

$$Q_{\text{xx}}^{\mathcal{C}}=l_{\text{xx}}^{\mathcal{C}}+(A^{\mathcal{C}}{)}^{\top }{V}_{\text{xx}}^{{}^{\prime }\mathcal{C}}{A}^{\mathcal{C}}\text{\hspace{1em}(8c)}$$

$$Q_{\text{uu}}=l_{\text{uu}}+(B^{\mathcal{C}}{)}^{\top }{V}_{\text{uu}}^{{}^{\prime }\mathcal{C}}{B}^{\mathcal{C}}\text{\hspace{1em}(8d)}$$

$$Q_{\text{ux}}^{\mathcal{C}}=l_{\text{ux}}^{\mathcal{C}}+(B^{\mathcal{C}}{)}^{\top }{V}_{\text{xx}}^{{}^{\prime }\mathcal{C}}{A}^{\mathcal{C}}\text{\hspace{1em}(8e)}$$

在每个时间步，这些$Q$矩阵可用于计算开环反馈项$k$以及闭环状态反馈增益$K$。

$$k_t=-Q_{\text{uu}}^{-1}{Q}_{\text{u}}\text{\hspace{1em}(9a)}$$
$$K_t^{\mathcal{C}}=-Q_{\text{uu}}^{-1}{Q}_{\text{ux}}^{\mathcal{C}}\text{\hspace{1em}(9b)}$$

最后，值函数需要更新：

$$V_{\text{x}}^{\mathcal{C}}=Q_{\text{x}}^{\mathcal{C}}-Q_{\text{u}}{Q}_{\text{uu}}^{-1}{Q}_{\text{ux}}^{\mathcal{C}}\text{\hspace{1em}(10a)}$$

$$V_{\text{xx}}^{\mathcal{C}}=Q_{\text{xx}}^{\mathcal{C}}-Q_{\text{u}}{Q}_{\text{uu}}^{-1}{Q}_{\text{ux}}^{\mathcal{C}}\text{\hspace{1em}(10b)}$$

iLQR-SVR使用MuJoCo模型进行前向展开，即使用完整的非线性系统动态。我们更新整个自由度集的状态向量，同时仅根据减少的自由度集计算控制修改，如方程12所示。

$$\widehat{{\text{x}}_0}={\text{x}}_0\text{\hspace{1em}(11)}$$

$$\widehat{{\text{u}}_t}={\text{u}}_t+\alpha k_t+K_t^{\mathcal{C}}({\hat{\text{x}}}_t^{\mathcal{C}}-{\text{x}}_t^{\mathcal{C}})\text{\hspace{1em}(12)}$$

$${\hat{\text{x}}}_{t+1}=f({\hat{\text{x}}}_t,{\hat{\text{u}}}_t)\text{\hspace{1em}(13)}$$

其中，${\hat{\text{x}}}_t$, ${\hat{\text{u}}}_t$表示新计算的轨迹状态和控制，$\alpha$是介于0到1之间的线搜索参数。如果找到更低成本的轨迹，标称状态和控制轨迹将从前向展开中更新。

备注1：如果某个自由度从$\mathcal{C}$中移除，那么在计算轨迹的运行成本时，该自由度仍会产生成本，即成本函数总是作用于完整的自由度集$\mathcal{F}$。

4.2 降维方法

在本节中，我们概述了如何识别需要从系统状态中删除的自由度（DoFs）。我们方法的核心思想是利用LQR增益矩阵$K$中的信息，这些矩阵在我们的情况下已经由iLQR算法计算并作为状态反馈增益矩阵返回。如果使用其他优化算法，则可以围绕名义轨迹应用线性化的LQR近似，以计算闭环状态反馈增益。

增益矩阵 K ≡ { K 1 , K 2 , … , K T − 1 } \text{K} \equiv \{K_1, K_2, \dots , K_{T-1}\} K≡{ K1​,K2​,…,KT−1​}用于计算闭环控制修正，当使用全非线性动力学进行演练时，将修正添加到名义控制向量中。通过计算新轨迹中的当前状态与为其计算导数的名义轨迹的差异来实现这一点。然后将这一差异与线性增益（即$K$矩阵）相乘。此过程在公式14中明确展示。

我们使用符号$K(.,.)$来表示矩阵内部的索引。$K$矩阵中的列对应于基于某个特定自由度与名义轨迹的偏差计算整个控制向量的修正。我们推断，如果某个自由度在整个轨迹上的增益值较小，那么它对整体轨迹优化问题的重要性就不高，可以忽略。这是因为即使该自由度与其名义位置有较大偏差，它对新计算的控制的影响也是最小的。

我们考虑了两种利用该信息的方法：a）简单地对矩阵中的相关值求和，b）执行奇异值分解（SVD），以试图找到最相关的自由度。两种方法都计算每个自由度的$dof\_importance$值，然后我们简单地使用一个阈值参数$\rho$来决定哪些自由度应该保留在$\mathcal{C}$中，哪些应该被删除。

求和方法 求和方法利用$K$矩阵中的值，评估每个自由度对控制向量修正的影响。如果在整个优化范围内某个自由度在$K$矩阵中的值较大，则该自由度很可能对当前的轨迹优化问题至关重要。我们为每个自由度$j$使用以下公式计算其$dof\_importance$值：

$$dof\_importance[j]=\sum _{t=0}^T\sum _{p=1}^m(K_t^{\mathcal{C}}(p,j)+K_t^{\mathcal{C}}(p,j+|\mathcal{C}|))/T\text{\hspace{1em}(15)}$$

我们在控制向量$m$的维度上求和，并且在整个优化范围内对所有矩阵求和。最后，我们还对自由度$j$的位移和速度元素进行求和。

SVD方法 SVD方法对状态反馈增益矩阵执行奇异值分解（SVD）：

$$K_t=U_t{\Sigma }_t{V}_t^{\top }\text{\hspace{1em}(16)}$$

其中$K$是大小为$m\times 2|\mathcal{C}|$的状态反馈增益矩阵，$U$是正交矩阵，大小为$m\times m$，$\Sigma$是大小为$m\times 2|\mathcal{C}|$的对角矩阵，$V$也是正交矩阵，大小为$2|\mathcal{C}|\times 2|\mathcal{C}|$。

$\Sigma$的对角元素是奇异值${\sigma }_0>{\sigma }_1>\cdots >{\sigma }_m$。我们使用前$g$个奇异值（在本工作中，$g=3$）和$V$中的奇异向量来提取状态向量中最重要的自由度。具体来说，我们为所有当前的自由度集$\mathcal{C}$计算$dof\_importance$值：

$$dof\_importance[j]=\sum _{t=0}^T\sum _{n=0}^g\left(\right(V_t(j,n)+V_t(j+|\mathcal{C}|,n)\left){\sigma }_n\right)/T\text{\hspace{1em}(17)}$$

我们使用符号$V(.,.)$来表示矩阵中的索引。值得注意的是，对于这两种计算$dof\_importance$值的方法，我们都对优化范围进行标准化，因此$\rho$不需要根据使用的优化范围进行调整。

在这两种方法中，我们使用$dof\_importance$值来确定Alg. 1中第10行的自由度删除决策。对于任何重要性值低于$\rho$的自由度，我们都会将其从当前的$\mathcal{C}$中删除。唯一的例外是机器人自由度，在任何方法中都不允许删除；即，机器人自由度始终出现在状态向量中。

5. 结果

我们在异步MPC框架下进行模拟实验，如算法1所述。我们提供了多种方法的实验结果。我们有两个基线方法：

• iLQR-Baseline：一种基线方法，在系统的全自由度（DoFs）集合上正常执行iLQR。
• iLQR-Rand：一种基线方法，我们在一个随机减少的自由度集合上执行iLQR。在每次优化迭代中，我们并不使用任何智能方法来确定哪些自由度被选择，而是随机抽取$\theta$个自由度（除了机器人自由度之外）来使用。

我们评估了三种与基线方法对比的其他方法：

• iLQR-Naive：该方法将我们的自由度集合减少到仅包含任务成本函数中直接考虑的自由度。
• iLQR-SVR-SVD：我们提出的动态改变自由度数量的方法，详见第4.2节-SVD。
• iLQR-SVR-Sum：我们提出的动态改变自由度数量的方法，详见第4.2节-Summing。

所有实验均在16核11代Intel® Core™ i7-11850H @ 2.50GHz的机器上进行，内存为32GB。

5.1 任务定义

我们选择了三个高维度的非抓取操作任务进行实验，设计这些任务的主要思想是，虽然这些任务在机械上并不是特别具有挑战性，但它们具有高维度，以测试我们的方法能否有效地确定解决任务所需的自由度。在某些状态空间非常大的任务中，我们需要以某个系数减慢模拟代理线程的速度，以保持一定的性能水平。所有任务都使用了Franka Panda机器人手臂，具有7个机器人关节（不包括控制向量中的夹爪）。

我们在下文中概述了这三个任务的总体描述以及其任务的超参数。$T$为优化的时间步长，$\Delta t$为模型时间步长。$Y$为任务的超时时间步长，即代理线程中直到任务完成的时间步长。最后，slowdown factor是代理线程与模型时间步长相比的慢速倍数。

• 障碍物任务：该任务的目标是将一个绿色圆柱推到目标区域（由绿色轮廓表示），同时尽量少地干扰一组干扰物（如图1所示）。任务的超参数如下：$T=80,\Delta t=0.004,Y=2000,$ Slowdown factor $=1$。

• 软体任务：该任务的目标是推动一个软体物体。此任务旨在将一个高维度的红色软体物体推到目标位置（如图2所示）。任务的超参数如下：$T=50,\Delta t=0.004,Y=1000,$ Slowdown factor $=3$。

• 软刚体任务：此任务的目标是将一个绿色圆柱推到目标位置（绿色轮廓），但在机器人末端执行器和目标物体之间有一个高维度的软体物体。这意味着优化器需要考虑软体物体与刚体之间的动态关系以完成任务（如图3所示）。任务的超参数如下：$T=100,\Delta t=0.004,Y=1000,$ Slowdown factor $=5$。

图 2. 显示软任务轨迹示例的快照序列。目标是将红色可变形物体推到地板上的绿色平面圆圈处。该系统的全部 DoF 数为 115。

图 3. 显示软刚性任务示例轨迹的快照序列。目标是将绿色圆柱体移动到目标区域（透明圆柱形区域），在机器人末端执行器和绿色圆柱体之间放置一个高维软体。该系统的全部 DoF 数为 154。

5.2 异步MPC结果

在本节中，我们评估了我们提出的方法在三个任务上的表现。此前，在方程5中，我们定义了$MPC\_\mathtt{cost}$，即系统实际执行的轨迹成本。我们使用该指标来衡量成功率。我们展示了我们的方法能够在考虑减少的状态向量的情况下降低$MPC\_\mathtt{cost}$，与基线方法相比取得更好的结果。这是因为尽管在仅考虑部分自由度时单次优化的表现可能会有所下降，但显著的优化时间节省减少了策略滞后带来的负面影响。

表1展示了异步MPC实验的结果。对于障碍物任务，我们进行了100次运行，对于软体任务和软刚体任务，我们进行了20次运行。表中的第一个数值为平均值，第二个数值为90%的置信区间。需要注意的是，为了避免个别数据点过度影响结果，我们删除了障碍物任务中的异常值。在该任务中，由于圆柱体被用作目标和干扰物，在某些情况下，如果一个圆柱体以较大的速度翻倒，它会滚动到离目标位置很远的地方，直到任务超时。出现这种情况时，我们删除了这些异常值。最后，我们将所有成本值相对于iLQR基线方法进行了归一化。

表 1. 三种操作任务的异步 MPC 结果。表中的数值是杂乱任务 100 次试验和软硬任务 20 次试验的平均值。第一个值为平均值，第二个值为 90% 的置信区间。在前三种方法中，DoFs 的数量是一个预设参数，而其他方法则是动态调整 DoFs 的数量。

结果显示，在所有三个任务中，我们的方法（iLQR-Naive、iLQR-SVR-SVD和iLQR-SVR-Sum）都能够实现比iLQR-Baseline更低的$MPC\_\mathtt{cost}$。iLQR-Naive在所有三个任务上的平均$MPC\_\mathtt{cost}$减少了27%。通过选择我们动态方法的最佳参数化，它们能够将$MPC\_\mathtt{cost}$降低14%。值得注意的是，单纯限制状态向量的大小并不能达到与我们智能方法相同的性能水平，因为iLQR-Rand仅将$MPC\_\mathtt{cost}$减少了6%。

这些结果在某种程度上被软刚体任务所偏向，在该任务中，iLQR-Naive方法明显优于其他所有方法。如果我们只考虑障碍物任务和软体任务，那么iLQR-Naive将$MPC\_\mathtt{cost}$降低了8.5%，iLQR-SVR降低了12%，而iLQR-Rand实际上将$MPC\_\mathtt{cost}$增加了4%。

这表明我们的K-informed方法在这两个任务中更加有效。这很合理，因为Naive方法总是将一些不一定重要的自由度包括在状态向量中。例如，在障碍物任务中，当某些物体没有直接或间接与目标物体/机器人接触时，考虑它们的动力学特性对轨迹优化解决方案没有任何价值。我们的K-informed方法能够自动推断出这一点。

需要注意的是，我们在这些结果中仅展示了三个$\theta$和$\rho$的参数化，并且对所有三个任务保持相同，以测试我们方法的通用性。如果这些值针对每个任务进行了特定的调整，可能会取得更好的结果。

我们还对修改$\theta$和$\rho$值在障碍物任务中的效果进行了实验，结果如图4所示。我们针对三个不同的$\theta$值进行了实验，并将$\rho$的值缩放至0到500之间（请注意，数值并非均匀间隔）。首先，当$\rho$设为零时，iLQR-SVR的操作与iLQR-Baseline完全相同。这在图4中清楚地表现出来，因为所有三个数据点的$MPC\_\mathtt{cost}$几乎相同。随着$\rho$值的增加，它使得我们的方法在选择哪些自由度保留在状态向量中时更加“挑剔”。这可以清楚地从底部的图表中看出，随着$\rho$的增加，状态向量中的自由度数量平均减少。

图 4. 上图显示了杂波任务 100 次运行的平均 MPC 成本。浅阴影区域显示了 90% 的置信区间范围。下图显示了状态向量中的 DoFs 平均数。我们使用了三种不同的 $\theta$ 参数和 $\rho$ 的比例值。

当$\theta$设为零时，没有任何自由度会被重新引入到我们减少的自由度集合中。这意味着一旦一个自由度被移除，它就永远不会有机会在优化中重新考虑。显然，这是一个糟糕的策略，因为它否定了我们方法的核心思想，即自由度的重要性会随着任务时间的推移而变化，图4中清楚地表明，当$\theta$值设为零时，性能明显比其他两个参数化的$\theta$差。

对于$\rho$的最佳参数化值应为中等值，因为当$\rho$接近零时，iLQR-SVR的性能应趋向于iLQR-Baseline，而当$\rho$趋向于较大的值时，iLQR-SVR的性能应趋向于iLQR-Rand。

6. 讨论

在本研究中，我们做出了几个贡献。首先，我们概述了一种在线改变状态向量大小以用于MPC的一般方法。其次，我们提出了一种智能方法来减少轨迹优化中考虑的自由度数量，并证明了在异步MPC格式下使用该方法可以提高性能。

然而，使用我们在本文中所述的阈值方法来决定是否将某些自由度包含在状态向量中的一个不幸结果是，阈值有时难以调整，并且可能依赖于任务。未来需要更多的工作来自动确定调整该参数的更好方法。一个可能更为稳健的方法是，在执行MPC之前预先确定一个所需的状态向量大小，而不是基于某个重要性阈值移除自由度。我们可以保留最重要的自由度，直到达到我们所需的状态向量大小。

在本研究中，我们仅考虑了减少状态向量大小的离散方法，即一个自由度要么被完全考虑，要么不被考虑。一个有前景的未来研究方向是考虑更传统的降维方法，通过将自由度组合到某个可以通过主成分分析（PCA）发现的低维空间中。

致谢

本研究得到了英国工程与物理科学研究委员会（EPSRC）资助，资助号为EP/V052659/1和2596473。为了实现开放获取，作者已为任何作者接受的手稿版本申请了Creative Commons Attribution（CC BY）许可证。