SEP-NMPC: Safety Enhanced Passivity-Based Nonlinear Model Predictive Control for a UAV Slung Payload System

本文提出了一种安全增强型无源非线性模型预测控制（SEP-NMPC）框架，通过将严格无源性不等式与高阶控制障碍函数（HOCBFs）相结合，为在复杂环境中运输吊挂负载的四旋翼无人机提供了兼具渐近稳定性与碰撞避免能力的实时控制方案。

要点

这篇论文介绍了一种名为 SEP-NMPC 的新技术，专门用来控制一种特殊的无人机：它下面挂着一个像秋千一样晃动的重物（比如一袋货物）。

为了让你更容易理解，我们可以把这项技术想象成**“一位既懂物理又懂交通规则的超级管家”，他在指挥一位“背着沉重且摇晃的背包的快递员”**穿过拥挤的人群。

以下是用通俗语言和比喻对这篇论文核心内容的解读：

1. 核心难题：背着“醉汉”走路

想象一下，你是一名快递员（无人机），你的背上挂着一个装满水的袋子（悬挂的负载），袋子是用绳子系着的。

• 普通无人机：就像背着背包走路，背包是固定的，很好控制。
• 挂绳无人机：就像背着一个喝醉了的朋友。你往左走，他往右晃；你急刹车，他往前冲。这种“摇晃”会让控制变得非常困难，而且如果周围有障碍物（比如电线杆或行人），不仅你要避开，那个晃来晃去的“醉汉”（货物）也不能撞到东西。

以前的控制方法要么只顾着让无人机飞得稳，要么只顾着避开障碍物，很难同时做到“既稳又安全”，尤其是在货物剧烈摇摆的时候。

2. 解决方案：SEP-NMPC（超级管家）

这篇论文提出的 SEP-NMPC 就像是一个拥有双重超能力的管家，他同时做两件事：

能力一：能量“消音器”（稳定性）

• 比喻：想象那个摇晃的货物是一个在秋千上乱晃的孩子。如果没人管，秋千会越荡越高，最后失控。
• 做法：这个管家手里有一个“能量消音器”。他通过一种叫**“无源性（Passivity）”**的数学原理，时刻监测货物的摆动能量。一旦货物开始剧烈摇晃，管家就会巧妙地调整无人机的动作，像给秋千加了一个阻尼器一样，把多余的晃动能量“吸走”并消耗掉。
• 结果：无论货物怎么晃，管家都能保证它最终停下来，并且稳稳地停在目标位置，不会越荡越远。

能力二：智能“安全气泡”（安全性）

• 比喻：想象无人机和货物周围都包裹着一个看不见的、有弹性的**“安全气泡”**。
• 做法：以前的方法可能只保护无人机本身（只保护快递员）。但这个方法很聪明，它知道货物会甩出去，所以它的“安全气泡”是包裹着“无人机 + 货物”整体形状的。
  • 如果货物甩向左边，气泡就会在左边变大。
  • 它使用了**“高阶控制障碍函数（HOCBF）”，这就像是一个超级灵敏的雷达**。它不仅看现在有没有撞，还能预测下一秒货物会不会甩出去撞到东西。
• 结果：即使货物正在剧烈摇摆，这个系统也能确保“无人机”和“甩出去的货物”都不会碰到周围的墙壁、电线或其他飞行的无人机。

3. 它是如何工作的？（实时决策）

这个系统不是死板的程序，而是一个每秒计算 100 次的超级大脑。

• 它每秒钟都在问自己：“如果我现在往左飞，货物会怎么晃？会不会撞到右边的树？”
• 它把所有可能的危险（碰撞）和所有可能的失控（摇摆）都写进一个**数学考试卷（优化问题）**里。
• 它不仅要考出高分（飞得准），还要保证不挂科（不撞车、不失控）。
• 关键点：这个计算过程非常快，而且不需要人工去调整参数（不需要“调音”），它自己就能算出完美的飞行路线。

4. 实验结果：真的管用吗？

研究人员在电脑模拟和真实的无人机上做了大量测试：

• 场景：让无人机穿过像迷宫一样的障碍物，或者躲避其他飞过来的无人机。
• 对比：他们把这种方法和其他旧方法（比如只懂避障不懂稳摆，或者只懂稳摆不懂避障）做对比。
• 表现：
  • 旧方法：要么货物撞墙了，要么货物晃得太厉害飞丢了，要么计算太慢来不及反应。
  • SEP-NMPC（新方法）：货物稳稳当当，无人机灵活地绕过障碍物，就像一位身怀绝技的杂技演员，一边背着晃动的重物，一边在拥挤的街道上优雅地跳舞，从未发生过碰撞。

总结

这篇论文的核心贡献就是发明了一种**“双管齐下”**的控制策略：

1. 用物理能量原理让摇晃的货物乖乖听话（稳）。
2. 用高级预测算法给整个系统穿上防弹衣（安）。

这让无人机运送重物穿过复杂、拥挤甚至动态变化的环境（比如城市物流、救援现场）变得更加安全、可靠且实时可行。简单来说，它让无人机从“笨拙的搬运工”变成了“灵巧的杂技大师”。

技术摘要

1. 研究背景与问题定义 (Problem Statement)

背景：
无人机（UAV）在物流、搜救和巡检等任务中应用广泛。其中，吊挂负载（Slung Payload） 是一种无需专用机械夹爪即可运输货物的灵活方式。然而，柔性缆绳引入了额外的自由度，使得系统成为欠驱动（Underactuated） 系统，且负载的摆动与飞行器动力学强耦合。

核心挑战：
在复杂（拥挤）环境中进行吊挂运输时，面临两大难题：

1. 稳定性（Stability）： 快速机动容易引发负载剧烈摆动，导致跟踪精度下降甚至系统失稳。现有的模型预测控制（MPC）方案往往缺乏闭环稳定性保证，或需要复杂的增益调度。
2. 安全性（Safety）： 负载的摆动会显著扩大系统的“有效几何尺寸”（Swinging Footprint）。现有的避障方法通常只考虑无人机机身，忽略了摆动负载可能碰撞障碍物，且缺乏对安全集前向不变性（Forward Invariance）的严格数学保证。

目标：
设计一种控制框架，能够在保证负载摆动抑制和系统渐近稳定的同时，确保无人机机身和摆动负载在静态及动态障碍物环境中均能安全避障，且具备实时计算可行性。

---

2. 方法论：SEP-NMPC 框架 (Methodology)

论文提出了一种安全增强型无源性非线性模型预测控制（SEP-NMPC） 框架。该框架将无源性理论（用于稳定性）与高阶控制障碍函数（HOCBFs，用于安全性）统一在一个二次规划（QP）兼容的优化问题中。

A. 系统动力学建模

• 建立了四旋翼无人机吊挂点质量负载的拉格朗日动力学模型。
• 系统具有 8 个自由度（机身位置、姿态、负载摆动角），但仅有 4 个控制输入（推力与力矩），呈现级联欠驱动结构。
• 采用内外环控制架构：内环（姿态）假设已完美跟踪，外环（位置）负责生成控制指令以稳定负载并规划轨迹。

B. 稳定性约束：基于无源性的能量整形 (Passivity-Based Stability)

• 能量存储函数（Storage Function）： 构建了一个增强的能量函数 $V(x)$，包含动能、势能（负载摆动能量）以及位置误差项。
  $$V(x) = \frac{1}{2}\dot{q}^T M(q) \dot{q} + m_L g l (1 - \cos\alpha \cos\beta) + \frac{1}{2}e_\zeta^T K e_\zeta$$
• 严格无源性不等式： 在 MPC 优化中嵌入严格无源性约束：
  $$u_a^T v \leq -\rho \|v\|^2 - \varepsilon \|u_a\|^2$$
  其中 $u_a$ 是整形控制输入，$v$ 是共置输出（速度），$\rho, \varepsilon$ 为增益。
• 作用： 该约束强制系统能量耗散，确保即使存在负载摆动，系统也能渐近收敛到期望平衡点，无需增益调度或启发式切换。

C. 安全性约束：高阶控制障碍函数 (HOCBFs)

• 双重安全集： 不仅考虑无人机机身（Quadrotor），还显式考虑负载（Payload）的位置。
• 相对阶处理： 由于控制输入通过加速度影响位置（相对阶为 2），标准的 CBF 无法直接应用。论文采用了二阶 CBF 递归构造。
• 前向不变性： 将安全约束转化为关于控制输入 $u_a$ 的仿射不等式：
  $$A_{CBF}(x, t) u_a \geq b_{CBF}(x, t)$$
  这保证了在静态和动态障碍物存在的情况下，机身和负载始终保持在安全距离内（前向不变性）。

D. 优化问题求解

• 将上述稳定性约束（无源性）和安全性约束（HOCBF）同时嵌入 NMPC 的优化目标中。
• 优化问题保持为二次规划（QP） 形式，可通过 SQP-RTI（序列二次规划 - 实时迭代）算法在线求解。
• 实时性： 无需增益调度，直接在线运行，适用于机载处理器。

---

3. 主要贡献 (Key Contributions)

1. 基于无源性的稳定性保证： 提出了一种嵌入 NMPC 的无源性存储函数，能够处理欠驱动四旋翼 - 负载系统的耦合动力学，从理论上保证了系统的渐近稳定性，无需复杂的参数整定。
2. 统一的安全包络： 设计了统一的安全约束，同时保证无人机机身和摆动负载在静态及动态障碍物环境中的最小距离分离，解决了传统方法忽略负载摆动范围的问题。
3. QP 兼容的实时框架： 提出了一种无需启发式切换的 SEP-NMPC 方案，将稳定性和安全性无缝集成，并在机载硬件上实现了实时运行（100Hz 控制频率）。

---

4. 实验结果与验证 (Results & Validation)

论文通过广泛的仿真和真实世界实验验证了该方法的有效性。

A. 仿真设置

• 环境： 包含静态圆柱障碍物和动态移动障碍物（模拟其他无人机）。
• 对比基线： 仅状态约束、一阶 CBF、高阶 CBF（无无源性）、仅无源性控制等。
• 指标： 避障成功率、负载摆动抑制、轨迹跟踪精度、求解时间。

B. 真实世界实验

• 平台： Quanser QDrone2 搭载 0.2kg 负载（缆绳长 0.5m）。
• 硬件： NVIDIA Jetson Xavier NX 机载计算。
• 性能数据（20 次单障碍物测试）：
  • 成功率： 20/20 (100%)。
  • 最小安全距离： 中位数 0.53m（满足安全阈值）。
  • 最大摆动角： 54.94°（在动态避障中有效抑制）。
  • 求解时间： 中位数 8.74ms（远低于 10ms 的控制周期，满足实时性）。
  • 无越界/不可行： 0 次失败。

C. 消融实验发现

• 无无源性： 即使使用高阶 CBF，系统在遇到不可行情况时容易产生超调（Overshoot）或振荡，导致收敛不稳定。
• 仅状态约束/一阶 CBF： 在复杂动态环境中频繁出现安全边界违反或求解器失败。
• SEP-NMPC（两者结合）： 表现出最佳的综合性能。无源性提供了平滑的耗散行为，防止了激进控制；HOCBF 提供了严格的安全保障。两者结合消除了超调，确保了平滑收敛和零碰撞。

---

5. 意义与结论 (Significance & Conclusion)

科学意义：
该工作成功解决了欠驱动吊挂系统在复杂环境中“稳定性”与“安全性”难以兼顾的难题。它证明了通过数学形式化（无源性 + HOCBF）可以将物理系统的能量耗散特性与几何安全约束统一在一个优化框架内，且不需要牺牲计算效率。

工程价值：

• 实用性： 算法在低算力机载硬件（Jetson NX）上实现了实时运行，证明了其在实际物流和救援任务中的部署潜力。
• 鲁棒性： 能够处理动态障碍物和剧烈的负载摆动，显著提升了无人机在拥挤环境下的作业能力。
• 通用性： 框架不依赖特定的障碍物类型，可推广至其他欠驱动系统。

总结：
SEP-NMPC 为无人机吊挂运输提供了一种形式化保证（Formal Guarantees） 的解决方案，实现了在确保负载不碰撞障碍物的同时，系统状态渐近收敛到目标点，是迈向自主空中物流系统的重要一步。