实时神经MPC：用于四旋翼和敏捷机器人平台的深度学习模型预测控制

自主系统的精确动力学建模是具有挑战性的。例如，平台接近高速、高加速度或与环境接触时。准确的建模对于自主飞行系统尤其具有挑战性，因为高速和高加速度可能导致复杂的空气动力学效应[1]，而在靠近障碍物的空中飞行中需要建模交互力，例如地面效应。数据驱动的方法，特别是神经网络，已经证明了准确建模高度非线性动态效应的能力[1]，[2]。然而，由于它们的计算复杂度较大，将这样的模型集成到嵌入式MPC管道中仍然具有挑战性，因为需要高频实时要求。为了克服这个问题，先前的工作依靠了以下两种策略之一：

(I) 大大减少模型的容量，这会使得许多预测性能丢失，但可以实现实时速度[2]-[6]。通常，模型被简化为具有少数支持点的高斯过程（GP）[3, 4]或小型神经网络[5]-[7]。但是，这些方法仅在强大的i7 CPU上的设备外部应用。

(II) 使用与在线优化MPC不同的控制策略，这些策略要么不具有预测性[8]，[9]，不使用在线优化[7]，[10]，[11]，要么端对端学习控制器[12]-[17]。

本文介绍了一种有效的框架，实时神经网络MPC（RTN-MPC），允许将大容量的数据驱动动力学模型集成到在线优化模型预测控制中，并在嵌入式设备上实时部署。具体来说，该框架使任意神经网络架构都可以作为动力学约束集成到MPC公式中。为此，RTN-MPC利用CPU或GPU并行化的本地逼近数据驱动模型。与将深度网络简单集成到MPC框架中相比，我们的方法允许选择任意模型架构，具有更大模型的嵌入式实时能力和GPU加速，而不会降低性能。

图1：嵌入式神经网络模型预测控制。将神经网络直接集成到 MPC 优化循环中会导致优化时间很长（红色）并导致不稳定性。我们的方法可以处理更复杂的大型学习模型，同时具备实时性能（绿色）。 

贡献

我们的贡献有三个方面：首先，我们制定了RTN-MPC的计算范式，这是一种使用深度学习模型进行预测步骤的MPC框架。通过将计算量大的数据驱动模型与MPC优化分离，我们可以利用高效的在线逼近方法，允许更大、更复杂的模型，同时保留实时性能。其次，我们比较并消除了在具有和不具有RTN-MPC范式的情况下的MPC问题，在CPU上展示了改进的实时性能，在GPU处理可用时更进一步增强了性能。最后，我们在多个基于模拟和实际的实验中评估了我们的方法，这些试验使用高速四旋翼在激进和接近障碍的操作中运行大模型，模型容量比最先进的算法高出几个数量级，且在实时窗口内实现。

据作者所知，这是第一种可以在敏捷平台上实时进行基于梯度的MPC设置中使用数据驱动模型的方法。此外，它可以扩展到大型模型，并在离线CPU或GPU上运行，使模型大小适合于闭环MPC[1]，[2]。所提出的框架虽然在敏捷四旋翼飞行器上进行了演示，但可以广泛应用，并有益于任何受控敏捷系统，例如无人驾驶车辆或机械臂。

本文的组织如下：在“相关工作”章节中，我们重新审视了相关文献，这些文献启发了我们的工作，并在“问题建立”章节中介绍了RTN-MPC。随后，我们在板载和非板载、CPU和GPU场景下评估了独立运行时的性能（“运行时的分析”章节）；最后，在“实验”章节中，我们展示了我们的方法在嵌入式实际系统中的可行性。

随着深度学习的出现，有大量的研究致力于将深度神经网络的表征能力与系统建模和控制相结合。接下来，我们简要概述一下之前的工作，重点关注基于学习的动力学建模、端到端控制器学习以及将数据驱动的动力学模型与传统控制流程相结合的方法。

数据驱动的动力学模型 。由于深度神经网络能够在大量数据中识别模式，因此它们是建模复杂动态的一种有前途的方法。先前的工作利用深度网络的表征能力进行建模任务，其中包括四旋翼[1]、[2]、[18]和直升机[19]的气动模型、湍流预测[20]、轮胎摩擦模型[21]和执行器模型[22]。虽然这些工作证明了神经网络可以学习到能够学习真实世界机器人系统的特殊性质的系统模型，但它们仅限于仅在仿真中使用的情况，或者将网络预测用作传统控制流程中的简单前馈组件：Saviolo等人[2]将准确的受物理启发的模型转换为简单的多层感知器，以用于闭环控制。

学习型控制器。随着深度学习和尤其是深度强化学习的兴起，出现了一种新的机器人系统控制器，它直接将传感器觉察到的信息映射到动作上。这些控制器可以通过模仿专家控制器的动作[12]，[13]，[16]或在模拟环境中进行广泛交互[23]来进行监督式训练。基于模型的强化学习(MBRL) [7]，[10]，[11]，[17]是另一种越来越受欢迎的方法，甚至可以学习低级控制器[11]。虽然MBRL方法可以像在线MPC一样以递归地时间规划方式应用[10]，但它们通常不依赖在线优化，并且例如从样本中启发式地选择行动序列[10]。虽然这些方法可以实现高控制频率，并可能优于在线MPC方法，但它们需要大量的训练数据集，不允许进行不带成本的调整，并且无论何时部署到分布不均匀的数据上，都无法保证在线MPC框架的最优性、鲁棒性和普适性。

数据驱动控制。近年来，将学习的模型与嵌入式控制框架结合的方法得到了广泛研究。大多数方法集中在将学习的模型与简单的反应式控制方案结合，例如“神经着陆器”方法[8]。神经着陆器使用了关于地效气动力学的学习模型，从而显著改善了近悬停状态下的设定点控制器。在[24]中，一个学习的循环动力学模型建立了一个基于模型的控制问题。虽然这种方法允许系统在线适应不断变化的操作条件，但它无法考虑到系统的约束，如有限的执行输入。最近的方法将数据驱动方法的建模能力与MPC框架相结合，提出了使用高斯过程（GP）作为赛车[4]和四旋翼[3]的学习剩余模型的方法。对于高斯过程，它们的复杂性和准确性都随着诱导点的数量和其维数而增加。嵌入式MPC通常需要采样时间低于50ms，这导致多维问题最多只能有10个诱导点[4]，而单维问题最多只能有20个[3]。由于预定义的固定核函数以及可行诱导点的数量较少，严重限制了基于GP的MPC方法的性能。Chee等人[5]遵循[3]的方法，但使用两层神经网络对四旋翼的动力学剩余进行建模。[7]结合了基于采样的MPC和由深度神经网络表示的动力学模型，不是使用GP学习剩余模型。虽然这种方法成功地应用于小型赛车，但所需的采样步骤限制了其应用于低维动作空间的平台。

在[6]中，将神经网络动力学模型与MPC相结合，用于无人驾驶赛车，在轮胎附着极限下展示了高性能驾驶。该方法在优化器中使用了完整的网络复杂性，这再次将网络架构限制为简单的两层模型，以保持实时操作。在表I中，我们比较了现有的基于数据驱动的MPC方法的建模能力。所有最先进的模型都受到支持点数量较少的GPs或小型二三层神经网络的建模能力的严重限制。

在最一般的情况下，模型预测控制（MPC）通过在考虑当前和未来时间步骤下对输入和状态变量的约束条件的情况下，找到一个输入命令，使得其系统动力学模型的代价函数最小化，从而解决了一个最优控制问题（OCP）。传统上，模型是从第一原理手动推导出来的，使用“简单”的微分代数方程（DAE），通常忽略了复杂的动力学效应，例如空气动力学或摩擦力，因为它们很难或计算成本很高。在前期的研究[2] – [5]中，我们将划分为第一原理DAE 和学习的数据驱动模型的数学组合，这使得更一般的模型扩展了DAE动力学模型的能力。为了解决上述OCP问题，我们将系统在时间轴上离散为时间步长为、时间跨度为的个步骤，使用多段打靶法[25]来近似问题，从而得到以下非线性规划（NLP）问题：

其中，表示初始状态条件，可以包含等式和不等式约束，例如状态和输入变量的限制。是数值积分例程，用于离散化动力学方程，通常使用4阶龙格-库塔算法，涉及次对动力学函数的求值。使用SQP（顺序二次规划）对NLP进行优化，其中是SQP迭代，。

本节中，我们提出了加速使用神经网络进行 MPC 控制的优化时间的关键概念。在 A 章节中，关键洞见是，学习到的动态系统的局部近似足以保持类似的性能，同时大幅提高优化问题的生成速度。这一洞见被应用于 B 章节中的三阶段嵌入式实时优化过程中。

A.局部近似连续二次规划

由于嵌入式优化求解器的进展，SQP已成为有效解决OCP的框架。这涉及将公式（1）反复逼近和求解为二次规划（QP）。QP的解决方案导致对迭代的更新，其中步长由以下QP求解给出：

受

其中，，线性化成本函数，且Hessian矩阵可以用高斯-牛顿算法来近似。和是对函数评估和的简写符号。主要的计算负担在于连续性条件方程（2）的参数计算。具体来说，对于每个打靶节点，我们需要计算：

导致动力学方程中需要进行 次偏导数计算：

和个函数进行计算：

对于计算密集型的数据驱动动力学模型，这会导致生成QP所需的处理时间很长。

学习得到的数据驱动动力学方程被假定在训练数据集中存在的状态和控制输入空间内是准确的。然而，为了创建QP连续性条件，我们只需要模型及其导数在当前迭代点附近准确即可。因此，为了加快QP生成速度，我们使用计算较轻的局部有效近似，其在当前迭代点附近的输入值处具有二阶精度。

对于一阶近似，所需的微分可作为的子矩阵轻松获取，对于二阶近似，所需的微分可作为张量乘法和求和的子矩阵获得。这种计算简化的误差在一阶近似中是二阶的，在二阶近似中是三阶的，与节点之间状态和控制变化的大小成比例，我们将在试验章节中证明，对于很小的敏捷平台而言，这种误差可以忽略不计。

使用公式（4），QP创建与数据驱动动力学模型的复杂性和架构无关。此外，由于和是SQP优化和数据驱动动力学模型之间的唯一接口，因此我们可以自由地优化近似过程，而不依赖于NLP框架；将它们作为参数传递给QP生成的连续性条件过程。由于是一个通常由大型矩阵乘法组成的神经网络模型，因此我们可以自由地使用为神经网络评估和微分优化的算法和硬件。这些功能在现代机器学习工具（如PyTorch [26]和TensorFlow [27]）中已经得到广泛应用。这使我们能够在CPU或GPU上作为一个单一并行批处理来计算所有打靶节点N的Jacobian矩阵和Hessian矩阵。

B.实时神经MPC

即使没有数据驱动的动力学模型，求解SQP直到收敛对于敏捷机器人平台也是不可行的。因此，受快速动力学影响的MPC应用通常使用实时迭代方案（RTI）[28]来解决，其中只执行单个SQP迭代——构造和解决为潜在的次优问题，但及时输入命令比最优的后期命令更可取。

如图2所示，RTN-MPC将实时优化过程分为三个部分: QP准备阶段、数据驱动动力学准备阶段和反馈响应。

对于可用的迭代，数据驱动动力学准备阶段使用CPU或GPU上的有效批量微分计算出和。

同时，QP准备阶段使用以数据驱动动力学准备阶段的结果为参数的连续性条件的一阶逼近，围绕和进行线性化来构建QP。

一旦感知到新的扰动状态，反馈响应阶段就使用扰动状态作为输入来求解预构建的QP。通过QP的结果调整迭代并将优化的命令发送到执行器。

C.实现

为了演示RTN-MPC范例的适用性，我们提供了一个使用CasADi [29]和acados [30]作为优化框架和PyTorch [26]作为ML框架的实现1(https://github.com/TUM-AAS/ml-casadi)。我们使研究者能够使用任意在PyTorch中可训练且在CasADi优化中可用的神经网络模型，如下所示：

此外，我们将把我们的RTN-MPC方法与[2]、[5]、[6]中应用的神经网络数据驱动MPC的直接实现进行比较。在这里，学习的模型以训练的权重矩阵和激活函数的形式直接在CasADi中构建。随后，CasADi中的QP生成和自动微分引擎必须处理完整的神经网络结构，因为它缺乏优化算法，而仅限于CPU。

与在线MPC中数据驱动动力学模型的朴素实现相比，我们展示了我们提出的RTN-MPC范式的计算优势。因此，我们构建了一个实验问题，其中名义动力学是微不足道的，而数据驱动的动力学可以在计算复杂性方面任意扩展。因此，名义动力学模型是在位置上的双积分器，而数据驱动动力学是一个可变架构的神经网络。为了仅关注数据驱动动力学的计算复杂性，而不是建模精度，网络没有进行训练，而是手动调整权重以强制输出为零。

我们使用的四阶显式龙格-库塔方法对进行数值积分。

在这个实验中，我们模拟系统，不考虑任何模型-实际系统不匹配的问题，仅关注运行时间。优化问题通过构建 个节点的多段打靶法来解决。

图3 评估不同双层模型参数容量的实时能力。我们在嵌入式平台（Nvidia Jetson Xavier NX）和笔记本电脑（Intel i7，Nvidia RTX 3000）上进行评估。参数模型容量由每层神经元的平方近似。RTN-MPC框架可以运行比朴素实现大4000倍的参数复杂度模型。为了使结果可比较，我们定义至少50Hz的目标运行时间窗口（红线），最好是100Hz以上（绿线）。但是，在实际的应用场景中，实时窗口是特定于用例的。

根据通用逼近定理，一个具有两个隐藏层的神经网络可以通过每层足够数量的神经元逼近任何函数。图3比较了具有不断增加的神经元数量的两层网络在朴素实现和我们的RTN-MPC框架中的表现。在嵌入式系统上，如Nvidia Jetson Xavier NX，我们的方法能够在CPU上实现60倍参数复杂度的更大模型，在GPU上则是4000倍，同时保持50Hz以上的实时响应时间。在桌面上运行，这是当前数据驱动MPC研究的默认设置[2]，[3]，[5]，[21]，我们可以在低端GPU上以超过100Hz的速度运行具有超过1.5亿参数的两层模型。

表II：朴素实现与RTN-MPC的运行时间比较。在参数容量达到约10K及以上时，朴素方法的运行时间变得不可行，控制频率下降到低于50Hz。我们的方法可以扩展到强大的网络和复杂的网络架构，对于在嵌入式设备上超过50K参数和在GPU上超过13M参数的实时控制频率均超过100Hz。

本文在“问题建立”章节描述的RTN-MPC框架可以应用于各种机器人应用，但我们将使用敏捷四旋翼飞行器来展示其在真实世界问题中的潜力。

符号说明： 标量用小写字母表示，向量用小写粗体表示，矩阵用大写粗体表示。坐标系，例如世界坐标系和机体坐标系用正交基表示，即，其中机体坐标系原点位于四旋翼飞行器的质心处（见图4）。在W坐标系中表示从坐标到的向量记为。如果向量的起点与其所在的坐标系重合，则省略坐标系索引，例如四旋翼位置表示为。使用单位四元数表示方向状态，其中，例如四旋翼机体的姿态状态。最后，完整的SE3变换，例如将一个点的参考系从机体坐标系转换为世界坐标系，用描述。请注意，四元数向量乘积用表示，用四元数表示向量的旋转，，其中是四元数的共轭。

名义四旋翼动力学模型。名义动力学假设四旋翼是一个具有6个自由度的刚体，质量为，惯性矩阵为对角矩阵。我们的模型类似于[3]，[31]，[32]，因为我们将名义动力学写为对一阶和二阶导数的微分方程，将四旋翼各自的旋翼推力作为控制输入。因此，状态空间是13维的，其动力学方程可以写成：

其中，，表示地球的重力，表示集体推力，表示机身力矩。同样，我们使用了显式的4阶龙格-库塔积分方法。

增强空气动力学残差模型。 在此我们采用了类似于之前的研究[3], [4]的数据驱动模型，即通过神经网络来补充名义动力学的模型残差。在完整的配置中，我们的残差动力学模型定义如下：

在上述公式中，我们单独考虑了未知于名义动力学的线性和角加速度扰动，并且和是分别模拟线性和角扰动的神经网络的参数。

我们还评估了残差模型的两个简化版本： 

这些简化模型仅考虑残余力与平台速度（左侧），可能伴随着命令输入（右侧）的函数关系。

增强地面效应模型。 为了展示我们的方法的强大之处，利用复杂的任意高级输入，我们使用在四旋翼下的高度图作为额外的输入来扩展残余模型，以对地面效应进行建模：

其中，是四轴飞行器的高度，是一个映射函数，它以四轴飞行器的位置和大小为的固定或感测到的全局高度图作为输入。该函数返回围绕四轴飞行器位置的一个局部高度图块，分辨率为10cm。

MPC成本函数形式化。 我们规定公式(1)中的成本函数为二次型，惩罚对参考轨迹、的偏差，并通过限制来考虑输入限制。

实验

在我们的实验中，我们将重新验证之前的研究[2]，[5]的发现，即在MPC中与没有数据驱动模型或高斯过程相比，使用神经网络数据驱动模型可以提高跟踪性能。然而，更重要的是，我们将证明RTN-MPC可以使用更大的网络容量，以充分利用可能的性能增益，同时提供实时性能。

所有实验分为两个阶段：系统识别和评估。在系统识别期间，我们使用MPC控制器中的名义动力学模型收集数据。状态-控制时间序列进一步处理为后续状态，控制元组。然后使用名义控制器重新模拟每个步骤，并将误差用作剩余模型的训练标签。

在评估阶段，我们跟踪两条固定的评估轨迹：圆形和双扭线，并基于参考位置跟踪误差来衡量性能。因此，我们报告参考轨迹与跟踪轨迹之间的（平均）欧几里德距离作为误差。

为了确定实验中使用的模型体系结构，我们使用一种命名约定，即模型类型后跟大小和实现类型，其中我们区分我们的RTN-MPC方法（-Ours）和朴素集成（-Naive）。N-3-32-Ours是一个具有3个隐藏层，每个隐藏层包含32个神经元的神经网络模型，使用我们的RTN-MPC框架，而N-3-32-Naive则使用简单集成。GP-20是具有20个感应点的高斯过程模型。

A. 仿真

我们使用两个仿真环境来比较非增强MPC控制器、数据驱动动力学的朴素集成[2]，[5]，[6]和GPs [3]的实时性能和模型容量，这两个环境特征不同的建模准确性和实时要求。

表III: 简化模拟实验的结果。即使使用小模型，我们的深度学习模型也优于高斯过程。对于大型模型，我们的RTN-MPC框架（-Ours）允许实时功能，而与朴素集成（-naive）相比，不会增加优化时间。

BEM四旋翼模拟。 除了简化的模拟设置外，我们还在基于叶片元动量理论（BEM）[1]的高精度空气动力学模拟器中评估了我们的方法。与简化的模拟设置相比，这种模拟可以准确地模拟每个旋翼产生的升力和阻力，根据当前平台自我运动和单独旋翼速度进行模拟。模拟器实时运行，并通过机器人操作系统（ROS）与控制器通信。我们的目标是实现100 Hz的实时控制频率。

B.真实世界

最终，我们通过使用物理四旋翼平台agilicious [33]执行一系列敏捷轨迹的实验来评估我们的方法在真实世界中的有效性。我们使用Jetson Xavier NX计算期望的集体推力和机身角速度控制指令，并通过低级PID控制器跟踪这些指令。所有的真实飞行实验都在提供精准姿态估计的测试场地内进行，该场地以每秒400帧的速度提供数据。与模拟实验一样，在不超过14米/秒的速度下比较圆形和双曲线轨迹的跟踪误差。我们将我们的方法与名义控制器、朴素集成和文献[3]中部署的高斯过程配置进行比较。这些实验的结果如表格Ⅳ所示，我们将位置跟踪性能提高了高达82%，而朴素集成由于优化时间过长而变得不稳定。此外，我们的性能也超过了高斯过程，提高了高达55%。

地面效应。 最后，我们展示了我们的方法对其他用例的通用性，使用高度图作为输入来建模地效的复杂气动特性（见“实验设置”章节）。我们在飞行区域放置了一张70厘米高的桌子，并通过多次紧靠桌子表面飞行来收集数据。在评估中，我们重复飞过桌子上的轨迹，以四旋翼重心的高度为80厘米为目标高度；在四旋翼最低点（电池）和桌面之间留下大约2厘米的间隙。为了独立评估我们的方法对地面效应的补偿性能，我们以两种配置来评估训练模型。首先是模型不知道高度图信息的配置（作为比较基准），其次是模型已知高度图信息的配置。在一个包含8次飞行的评估轨迹中，在靠近桌子的位置（桌面xy平面+10厘米），我们将z方向的跟踪误差提高了72%。在图5中可以看到单次飞越的可视化效果。

在这项工作中，我们展示了一种方法，利用神经网络将数据驱动的模型预测控制的建模能力扩展到更大、更强大的架构，同时保持嵌入式设备的实时性能。我们的框架可以通过增加可用的实时建模能力，改进数据驱动的 MPC 的新应用和现有应用，从而使我们的方法适用于各种控制应用。

这项工作尚未考虑到的一个开放性挑战是，利用历史状态序列和控制输入在学习的动态模型中进行建模。这将自然地导致在优化循环中使用顺序和时间模型（如 LSTMs、GRUs 和 TCNs）来采用我们的方法，并引发在嵌入式 MPC 实时中目前仅在仿真中可行的方法[1]。

我们的实验表明，控制器的性能不会受到实时近似的负面影响。因此，该方法克服了以前工作中所描述的为了效率而牺牲性能的限制[2],[5]。我们通过评估 RTN-MPC 在不同设备上的隔离实时能力，并将该框架应用于高度敏捷四旋翼轨迹跟踪的具有挑战性的问题上，显著降低跟踪误差，同时可以在设备上使用强大的模型，展示了其有用性。