论文阅读： Learning to Plan Maneuverable and Agile Flight Trajectory with Optimization Embedded Network

可微分优化问题
https://arxiv.org/abs/2405.07736
https://github.com/ZJU-FAST-Lab/e2e_opt

Learning to Plan Maneuverable and Agile Flight Trajectory with Optimization Embedded Networks

💡摘要

近年来，越来越多的研究人员致力于利用深度神经网络进行端到端飞行导航。这种方法之所以受到关注，是因为它能够弥合传统方法中感知与规划之间的差距，从而消除模块之间的延迟。然而，以黑箱方式用神经网络替换原始模块的做法会降低整个系统的稳健性和稳定性。它缺乏原则性的解释，往往无法持续生成高质量的运动轨迹。此外，这种方法往往难以严格考虑机器人的运动学约束，导致生成的轨迹无法令人满意地执行。在这项工作中，我们结合了传统方法和神经网络的优势，提出了一种嵌入优化的神经网络。该网络可以直接从视觉输入中学习高质量的轨迹，而无需映射，同时确保动态可行性。在这里，深度神经网络用于直接从深度图像中提取环境安全区域。随后，我们采用基于模型的方法将这些区域表示为轨迹优化中的安全约束。利用高效的优化算法，我们的方法可以稳健地收敛到满足各种用户定义约束的可行和最佳解决方案。此外，我们区分了优化过程，允许将其作为神经网络中的一个层进行训练。这种方法促进了感知和规划之间的直接交互，使网络能够更多地关注存在最佳解决方案的空间区域。因此，它进一步提高了生成轨迹的质量和稳定性。

📉Introduction

无人机 (UAV) 因其紧凑的硬件设计和灵活的机动性，已在航空摄影、勘探和搜索救援等各个社会领域得到广泛应用。高效而强大的导航模块在实现无人机自主飞行方面起着至关重要的作用，引起了学术界和工业界的极大关注。

传统导航任务可分为感知和运动规划。感知模块处理来自深度摄像头等传感器的原始数据，以构建占用图并得出有利于运动规划的环境表示，例如欧几里得有符号距离场 (ESDF) (1) 或神经辐射场 (NeRF) (2)。另一方面，运动规划模块利用构建的地图以及机器人状态、运动学和避障约束来计算具有时间正则化的能量最小化轨迹。虽然传统导航策略提供了直观的工程解决方案以及理论完整性和可解释性，但它们在模块化分解框架中存在延迟，从而对四轴飞行器的敏捷飞行能力产生不利影响 (3)。此外，分层框架可能导致子模块之间缺乏互连，使飞行性能容易受到传感器噪声的影响，并且通常需要工程师手动微调大量参数 (4)。

近年来，人们越来越关注将感知和规划模块直接融合到单个神经网络中。这种端到端管道可以直接从传感器数据中学习运动，绕过显式映射。然而，这种策略会导致黑盒导航系统，使调试变得困难。此外，在动态可行性、避障和高质量轨迹生成之间取得良好的平衡会给网络带来沉重的负担。例如，由于物理平台或任务限制，通常需要对轨迹施加动态约束，例如最大速度和加速度约束。为了解决这些问题，研究人员通常会求助于为网络设计复杂的策略，这可能会影响最优性，并且仍然无法保证轨迹的动态可行性。

在这项工作中，我们结合了传统轨迹优化和神经网络的优势，开发了一种端到端视觉导航系统，该系统能够直接从深度信息生成轨迹，而无需显式映射。与传统的基于学习的运动规划算法相比，我们方法的一个关键特点是将数值优化与联合训练相结合。这种方法减轻了网络的负担，增强了系统的可解释性，并确保了最佳且动态可行的运动轨迹。此外，我们的方法具有可扩展性，允许包含额外的用户定义约束，而无需重新训练。在我们的技术方法中，我们利用神经网络直接从深度信息中提取安全引导区域，然后将其转换为轨迹优化期间考虑的几何空间约束。轨迹优化过程将这些约束作为安全边界，同时集成用户指定的动态约束，从而高效收敛（约 1 毫秒）到高质量轨迹。这确保了四轴飞行器即使在复杂环境中也能保持机动性和敏捷性。与传统的基于学习的黑盒导航系统不同，我们的优化算法基于清晰的数学模型，在网络生成的可行拓扑空间内稳健地收敛到最优解。此外，通过使数值优化可微分，可以将其建模为一个层并与神经网络联合训练。这使得轨迹评估损失的梯度可以直接反向传播到网络，使网络能够专注于最优轨迹所在的空间区域。此外，为了确保在实际飞行过程中对环境进行充分的探索，我们在网络中引入了运动基元。网络输出每个运动基元的选择概率，并根据这些概率为某些基元分配安全和可行空间。在实际应用中，我们可以在高概率运动基元所代表的安全空间内并行执行轨迹优化，并选择最优轨迹作为执行计划。本文的主要贡献可以总结如下：

• 我们设计了一个轻量级神经网络，能够直接从深度信息中识别有价值的运动基元，然后使用它们生成必要的安全空间约束，以便进行后续的数值优化。

• 通过使轨迹优化可微分，我们将其视为一个层并与网络联合训练。这种无缝集成使神经网络能够直接向最佳轨迹发展，消除了优化和学习过程之间的任何差距。

• 通过利用神经网络和数值优化的优势，我们提出了一种高质量、无地图的规划方法。这种方法能够即时生成最佳且安全的轨迹，严格遵守动态约束。

❓Related Work

A. 经典运动规划算法

基于梯度的运动规划（5–10）被广泛用于生成无人机的局部轨迹，将问题视为受约束的非线性优化。此类方法通常需要通过深度信息明确构建环境，然后手动设计策略以提取轨迹优化的安全约束。欧几里得有符号距离场（ESDF）被广泛用于建模安全约束，因为它们提供从任何网格点到空间内障碍物的有符号距离和梯度信息（5、6）。然而，构建 ESDF 会产生额外的计算成本，并且需要在效率和准确性之间进行权衡，因为更高的分辨率会成倍增加计算和内存需求。Zhou 等人（7）提出了著名的自我规划器，它通过在迭代框架内不断生成安全引导路径来提供避障梯度，从而避免了 ESDF 构建。然而，这种方法缺乏收敛保证，可能容易陷入不安全的局部最小值，尤其是在复杂环境中。此外，使用引导路径来变形轨迹会偏离原始轨迹优化问题的公式，从而影响最优性。基于走廊的方法（9、10）在局部运动规划领域也越来越受欢迎。这些方法使用几何计算从环境点云中提取可行的凸包，将安全约束建模为线性或锥约束。然而，这些方法需要额外的无碰撞路径来为凸包提供种子点。此类路径通常使用低维搜索算法（如 A* 或混合 A*）获得。这些搜索算法通常不考虑机器人的高阶运动学，导致凸包不利于生成可机动且动态可行的轨迹。

B. 基于学习的运动规划算法

基于学习的方法(3,4,11–16)已成为局部规划领域的有前途的方法，它无需显式映射并减少了延迟。Loquercio等人(3)利用深度卷积神经网络从深度图像中学习飞行轨迹，并以人类飞行员的轨迹作为监督。然而，这种方法需要高质量和大规模的数据集。Kulkarni等人(15)采用强化学习实现端到端导航，并通过自定义深度碰撞编码器增强安全性。这些方法严重依赖于神经网络的功能，缺乏关于运动可行性和轨迹最优性的原则性保证。最近，一些方法将网络与数值优化相结合。例如，一项特定的工作(16)使用网络学习了空间中任意一点的碰撞概率，然后将其进一步建模为轨迹优化中的安全约束。类似地，另一项工作 (4) 通过将机器人的形状建模为凸包并使用神经网络预测船体与最近障碍物之间的有向距离来解决更精细的避障问题。虽然这些工作将网络和优化集成到机器人导航中，但与我们的方法相比，一个显着的区别是网络和优化是上述工作中的独立组成部分。相比之下，我们的算法将可微分优化作为网络训练过程的一部分。因此，我们的方法促进了网络向有利于后续优化的方向发展，最终生成更高质量的解决方案，同时确保机动性和敏捷飞行

最近的研究也应用了双层优化的概念。陈等人（17）提出了 IA* 算法，该算法与传统的 A* 搜索算法不同，将其嵌入神经网络中进行训练。然而，该算法生成的路径由离散的网格点组成，因此不适合高速无人机直接跟踪。此外，该算法需要全局地图作为输入，而我们的方法则直接利用视觉信息。与我们的工作类似，Iplanner（13）是一个端到端的局部规划器，它直接从深度图像输出轨迹，并已广泛应用于四足机器人。在实现过程中，该方法使用闭式三次样条函数对网络输出的点进行插值，近似于考虑避障的原始轨迹规划问题的次优解。虽然这种方法很轻量，但它不能保证严格遵守用户定义的约束，从而限制了它在必须严格满足约束的场景中的应用。例如对于四旋翼飞行器，我们需要施加推力约束，这对于确保高速稳定飞行至关重要。

📚Method

A. 端到端导航系统概述

在本文中，我们采用一类独特的轨迹，称为 M IN CO (18)，来表示飞行轨迹 ξ。M IN CO 是一种特殊的多段多项式表示，其参数为段持续时间 T = [T1, T2, …, TN ]T ∈ N+ 和航路点 q = [q1, q2, …, qN−1] ∈ R3×(N−1)，其中 N 表示轨迹段的数量。这种紧凑的表示自然满足起点和终点处的状态约束，以及航路点处相邻多项式的高阶连续性，而无需额外的约束。基于这种表示，轨迹优化被公式化为具有一阶时间正则化的控制能量最小化，可表示如下：

其中 W 是正定能量权重矩阵。ρ 是时间正则化权重，u 表示控制变量的维度。G 表示预定义的运动学约束，这些约束是根据用户要求和机器人动力学专门制定的。此外，等式 (3) 表示基于飞行走廊 F 的避障约束。通常，该约束可以准确地建模为相对于机器人位置坐标的线性或圆锥约束。由于优化领域的进步，一旦轨迹优化等式 (1-3) 完全制定和建模，就可以使用成熟的基于梯度的数值方法，这些方法可以高效地以较低的计算成本收敛到高质量的解决方案。然而，在实际飞行过程中，从复杂而杂乱的环境中准确提取可行空间是一项重大挑战。如第 I 节所述，它涉及传感器数据处理、映射、图形搜索等多个模块，并且易受噪声影响，这使得这一方面成为整个导航系统的致命弱点。因此，在这项研究中，我们直观地探索了利用神经网络直接从深度图像中学习飞行走廊，而无需进行显式映射。本质上，我们使用神经网络在轨迹优化过程中学习安全约束。

我们的导航框架如图 1 所示，大致可分为用于提取飞行走廊的网络层和可微分轨迹优化层。网络层接收深度测量、机器人的当前状态和目标点的位置等输入，以输出飞行走廊。随后，轨迹优化层在严格遵守指定动态约束的同时，在飞行走廊内规划高质量的时空最优轨迹。此外，每个模块的输入和输出（例如目标点、飞行走廊和规划轨迹）都根据机器人的当前车身框架进行归一化。这种归一化增强了模型的泛化能力，并减少了整个导航系统对全局定位的依赖。在以下章节中，我们将首先介绍网络层的结构设计，并详细描述其输出。然后，我们讨论了轨迹优化层产生的最优解相对于空间约束的梯度传播，这为联合训练网络内的嵌入式优化奠定了基础。

B. 基于学习的安全走廊提取层

在深入研究网络结构的具体细节之前，我们首先实例化其输出表示。对于连续时间约束公式 (3)，与方法 (19) 类似，我们将多项式的每个部分离散为 λ 个约束点。随后，我们通过在这些约束点施加约束来控制整个轨迹：

其中φ为神经网络的参数。式（4）的物理意义在于神经网络需要为轨迹上的每一个约束点分配一个安全凸包，因此网络需要输出总共Nλ个凸包。另外，需要强调的是，神经网络理论上可以输出任意形状的凸包，但为了简单起见，便于理解，我们将每个凸包定义为一个由中心和长度参数化的立方体。

在这里，我们设计了一个基于运动基元的新型神经网络，使我们能够满足上述要求。该网络最初采用深度卷积和全连接层从深度图像以及机器人的初始和最终状态中提取特征。这些特征被融合以获得潜在表示，表示为 M。随后，M 通过神经网络进一步处理以输出预建运动基元 ζ 库的概率分布。值得一提的是，为了与后续走廊参数对齐，每个运动基元 z 都由 N λ 个点表示。此外，选定的运动基元 z∗ 和 M 被联合输入到最终的走廊生成模块中。它的作用之一是改进运动基元以提高准确性，修改后的点坐标作为安全立方体的中心。此外，该模块负责为每个立方体分配相应的长度。值得注意的是，这种基于运动基元的结构的优势至少在两个方面显而易见。首先，与直接回归最终立方体中心相比，我们的方法首先获得概率分布并选择更好的运动基元。这个过程可以建模为分类问题，从而减轻网络负担并促进学习和收敛。其次，在实际部署过程中，我们可以根据概率灵活地选择多个运动基元。这允许并行优化多个轨迹，从而增强机器人在不同环境拓扑中的探索能力。同时，它还提高了系统的容错能力。

关于动作基元库的构建，在实际实验中，我们使用经典导航算法记录了数万条轨迹，将这些轨迹均匀离散为 N λ 个点，并变换到局部身体坐标系中。为了消除不必要的重复动作基元并限制库的大小，我们对所有动作基元数据的端点距离和方向进行了规范化。最后，我们使用 k-means 算法对处理后的数据集进行聚类，并收集了大约 100 个精英动作基元作为库。

C. 可微分数值优化层

在本节中，我们将讨论如何使优化过程可微分，从而使我们能够在训练期间将应用于轨迹的损失梯度反向传播到网络参数上。为简单起见，我们将优化变量表示为 x = (q, T)。然后，具有不等式约束的非线性优化问题方程 (1-3) 可以一般地重新表述如下：

这里，F 表示包含原始约束公式 (2,3) 的通用公式，φ 表示神经网络参数。假设 x∗ 是该优化问题的最优解，L 是训练期间应用于轨迹的评估损失，则神经网络的梯度可以按如下方式计算：

通常，项 ∇xL 可以通过分析计算得出。因此，我们现在将重点转移到讨论 ∇φx 的估计。由于使用了基于梯度的数值求解器，一种估计参数梯度的直观方法（称为展开（20-24））涉及在整个迭代过程中维护整个计算图。但是，这种方法在内存使用和效率方面带来了重大挑战，尤其是在处理复杂问题公式时。此外，它还可能面临与梯度发散或消失相关的问题。在这项工作中，我们假设可以有效获得问题的最优解 x∗，因此采用了 Dontchev 和 Rockafellar 的隐函数微分定理（25）。该方法依赖于利用优化问题的一阶最优性条件（26、27）来分析估计参数的梯度，而无需显式展开整个迭代过程。该优化问题的拉格朗日函数如下：

那么相应的KKT（Karush-Kuhn-Tucker）条件如下：

其中 D(·) 表示向量的对角矩阵。然后，我们将全微分算子 d 应用于 KKT 条件下的方程：

随后，将式（10）进一步变形为紧凑的矩阵形式：

通过求解该方程组，我们可以分析获得所需的雅可比矩阵∇φx，进而可以推导出最终的参数梯度∇φL。

❗Experimment

A. 消融实验

为了验证将数值优化嵌入神经网络的有效性，我们在包含一万多条数据的测试集上进行了烧蚀实验，定量比较了神经网络提取的引导区域的安全性和优化轨迹的能量消耗，如表1所示。当引导区域的边缘碰到任何障碍物时，我们认为它是不安全的。此外，利用轨迹的加加速度平方对时间的积分来衡量能量消耗。

由于我们去除了对真实值的模仿，并在神经网络中加入了嵌入数值优化后最小化轨迹能量的损失函数，网络提取出的安全区域很快向着降低能耗的方向调整。同时，如上一节所提到的，通过能量损失函数的反向传播，我们可以去除安全区域尽可能大的要求，使得区域的安全约束更容易满足，让网络更加专注于包含最优解，从而得到更高的安全率。

B. 模拟实验

我们将算法部署在模拟四轴飞行器上，在如图2所示的动力学模拟环境中进行实验。模拟环境大小为50m×50m，其中包含随机生成的柱状和环状障碍物。在已知定位的情况下，我们要求四轴飞行器从地图边缘的随机位置出发，不断重新规划以穿越整个未知环境。

此外，我们在目标函数值和总处理延迟方面对所提出的方法与最先进的算法 Ego-planner (7) 进行了定量比较，进行了超过四千次比较测试。所有模拟均在配备 Intel i9-10900K CPU 和 Nvidia GeForce RTX3070Ti GPU 的台式机上运行。

在表 2 中，我们的方法取得了比 Ego-planner (7) 更好的性能。使用无碰撞轨迹作为基本事实进行监督，并基于轨迹优化结果的反向传播进行训练，使得神经网络给出更容易包含最优解的安全区域。因此，所提出的算法在目标函数值方面具有更大的优势。与传统管道相比，神经网络具有更稳定的推理时间和输出，从而带来较低的标准偏差。此外，端到端管道和基于 GPU 的并行推理使其具有更低的延迟。

⛏️Conclusion

在本文中，我们提出了一种新颖的规划框架，用于四旋翼飞行器的机动性和敏捷性。使用嵌入式优化神经网络，我们直接从视觉测量中规划轨迹，而无需显式映射，同时能够保证轨迹的动态可行性。得益于可微分轨迹优化，神经网络的负担得到减轻，可以更轻松地提取包含最优解的安全区域。与最先进的方法相比，模拟实验证明了该流程的效率。未来，我们将在复杂环境中的高速飞行中进行更多定量实验，在这种情况下，由于高延迟，经典方法的成功率将大幅下降。我们还将进一步优化所提出的流程，并与其他基于学习的方法进行更多比较。此外，该框架将部署到真实的四旋翼飞行器上进行更多测试。