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

Imagine you are a delivery drone, but instead of carrying a package in a box, you are carrying a heavy sack of flour hanging from a rope underneath you. This is a quadrotor with a slung payload.

Now, imagine you have to fly through a crowded room full of people (obstacles) who are walking around. This is the challenge the paper tackles.

Here is the problem:

The Swing: If you turn too fast or stop suddenly, the sack of flour swings wildly like a pendulum. This makes it hard to control and dangerous because the "effective size" of your delivery system becomes huge when the sack swings out.
The Crash Risk: If you don't account for that swinging sack, you might crash into a person or a wall, even if your drone body is far away.
The Math Problem: Most computer controllers are good at one thing or the other. They are either good at keeping the system stable (stopping the swing) OR good at avoiding obstacles. Doing both at the same time, especially in real-time, is incredibly hard.

The Solution: SEP-NMPC

The authors created a new "brain" for the drone called SEP-NMPC. Think of it as a super-smart co-pilot that combines two different philosophies to keep the flight safe and smooth.

1. The "Energy Sponge" (Passivity-Based Stability)

Imagine the swinging sack is like a child on a swing. If you push them at the wrong time, they go higher. If you push them at the right time, you can calm them down.

The Analogy: The controller acts like a giant energy sponge. Every time the sack starts to swing too hard (gaining too much "energy"), the controller instantly soaks up that extra energy and turns it into heat (dissipation).
The Result: Instead of the sack swinging wildly, the controller gently damps the motion. It guarantees that no matter how bumpy the ride gets, the system will eventually settle down and stop swinging. It doesn't just guess; it mathematically proves the energy will always go down, ensuring the drone doesn't spin out of control.

2. The "Invisible Force Field" (High-Order Control Barrier Functions)

Now, imagine you are walking through a crowd. You need to keep a safe distance from everyone. But here's the catch: you aren't just a point; you are a person plus a long rope with a sack at the end.

The Analogy: Standard safety systems might just draw a circle around the drone. But this system draws a giant, invisible, 3D bubble that covers the drone and the entire arc where the sack could possibly swing.
The "High-Order" Magic: Most safety systems look at where you are right now. This system looks ahead. It asks, "If I keep going this way for the next second, will the sack hit the wall?" It calculates the path of the swinging sack before it happens.
The Result: It creates a "force field" that the drone and the sack are mathematically forbidden to cross. Even if a person runs toward the drone, the system calculates a path that keeps the entire system (drone + swinging sack) safe.

How They Work Together

The genius of this paper is putting these two ideas into a single Optimization Problem (a fancy math puzzle the computer solves 100 times a second).

The Goal: Get from Point A to Point B.
Constraint 1 (The Sponge): "You must keep draining energy so the sack stops swinging."
Constraint 2 (The Force Field): "You must never let the drone or the sack touch the invisible safety bubble around obstacles."

The computer solves this puzzle instantly. It doesn't have to choose between being safe or being stable; it does both simultaneously.

The Real-World Test

The authors didn't just simulate this on a computer; they built a real drone with a hanging weight and flew it through a lab with obstacles.

The Competition: They compared their new system against older methods.
- Old Method A: Good at avoiding walls, but the sack swung wildly and crashed.
- Old Method B: Good at stopping the swing, but it crashed into walls because it didn't account for the swing's reach.
- The SEP-NMPC: It flew smoothly, the sack barely moved, and it never got too close to the obstacles. It solved the math puzzle fast enough to run on a small computer attached to the drone.

In a Nutshell

This paper gives a flying robot a superpower: the ability to carry a wobbly, swinging load through a crowded, dangerous room without crashing, without losing control, and without needing a human to take over. It does this by teaching the robot to "soak up" bad energy and to see a giant safety bubble around its entire swinging body.

Here is a detailed technical summary of the paper "SEP-NMPC: Safety Enhanced Passivity-Based Nonlinear Model Predictive Control for a UAV Slung Payload System."

1. Problem Statement

The paper addresses the challenge of controlling a quadrotor UAV transporting a cable-suspended (slung) payload through cluttered environments containing both static and dynamic obstacles.

Dynamic Complexity: The system is underactuated (8 degrees of freedom with only 4 control inputs) and exhibits strong coupling between the quadrotor's motion and the payload's swinging dynamics. Rapid maneuvers induce payload oscillations, degrading tracking accuracy and increasing collision risks.
Safety Limitations: Existing Model Predictive Control (MPC) approaches often fail to provide formal guarantees for both stability and safety simultaneously. Many methods ignore the "swept volume" of the swinging payload, leading to potential collisions even if the quadrotor body avoids obstacles. Others rely on heuristic switching or lack closed-loop stability proofs.
Goal: To develop a control framework that guarantees asymptotic stability (suppressing payload swings and tracking the reference) and formal safety (maintaining clearance for both the vehicle and the swinging payload) in real-time.

2. Methodology: SEP-NMPC

The authors propose Safety Enhanced Passivity-Based Nonlinear MPC (SEP-NMPC), a unified optimization framework that integrates two distinct theoretical guarantees into a single Quadratic Program (QP).

A. System Dynamics

The system is modeled using Lagrangian formulation, separating dynamics into:

Translational (Outer-loop): 5 DOFs (quadrotor position $x,y,z$ and payload swing angles $\alpha, \beta$ ).
Rotational (Inner-loop): Fully actuated attitude dynamics.
The control objective is to stabilize the system to a desired position while driving swing angles and velocities to zero.

B. Stability via Passivity-Based Constraints

To ensure stability without gain scheduling, the authors embed a strict passivity inequality directly into the MPC cost function constraints:

Shaped Energy Storage Function: A Lyapunov-like function $V(x)$ is constructed, combining the kinetic/potential energy of the system with a position error term.
Passivity Inequality: The optimization enforces $u_a^T v \leq -\rho \|v\|^2 - \epsilon \|u_a\|^2$ , where $u_a$ is a shaped control input and $v$ is the collocated output (velocity).
Effect: This constraint forces the system to dissipate energy, ensuring asymptotic convergence to the equilibrium point (desired position with zero swing) via LaSalle's Invariance Principle.

C. Safety via High-Order Control Barrier Functions (HOCBFs)

To handle the complex geometry of the swinging payload, the authors employ HOCBFs:

Dual-Body Clearance: Safety constraints are defined for both the quadrotor body ( $p_Q$ ) and the payload ( $p_L$ ). The payload position is calculated based on the quadrotor position, cable length, and swing angles.
Relative Degree Handling: Since the control input affects acceleration (second derivative of position), standard first-order CBFs are insufficient. The authors construct a second-order recursion to derive affine inequalities in terms of the control input $u_a$ .
Forward Invariance: These constraints guarantee that the "safe set" (distance to obstacles > safety margin) is forward-invariant, meaning the system cannot enter a collision state if it starts in a safe state.

D. Optimization Formulation

The resulting Nonlinear MPC problem is formulated as a Sequential Quadratic Program (SQP):

Objective: Minimize tracking error and control effort.
Constraints: System dynamics, actuator limits, swing angle limits, the passivity inequality (stability), and HOCBF inequalities (safety).
Real-Time Feasibility: Because the HOCBF constraints are affine in the control input, the sub-problem remains a QP, solvable efficiently on onboard hardware.

3. Key Contributions

Unified Stability and Safety: A novel framework that simultaneously enforces asymptotic stability (via passivity) and collision avoidance (via HOCBFs) without heuristic switching.
Payload-Aware Safety: Explicitly models the "swept volume" of the swinging payload, ensuring safety for both the drone and the load, unlike methods that only consider the drone body.
QP-Compatible Real-Time Control: The formulation allows for online solution at 100 Hz on lightweight processors (e.g., NVIDIA Jetson) without gain scheduling.
Formal Guarantees: Provides mathematical proofs for asymptotic convergence and forward invariance of safety sets.

4. Results and Validation

The framework was validated through extensive simulations and real-world experiments on a Quanser QDrone2 with a 0.2 kg payload.

Simulation & Experiments:
- Tested in scenarios with static cylindrical obstacles and dynamic moving obstacles (other drones).
- Performance: The system successfully navigated cluttered environments, suppressed payload swings, and maintained safe distances from obstacles.
- Real-Time: The solver achieved a median execution time of 8.74 ms (well within the 10 ms cycle time), confirming real-time feasibility.
Comparative Analysis (Ablation Study):
- Compared against baselines: State-constraints only, 1st-order CBFs, and HOCBFs without passivity.
- Findings:
  - Methods without passivity suffered from overshoot and instability when encountering infeasible states.
  - Methods without HOCBFs (using 1st-order CBFs) frequently violated safety boundaries or failed to solve.
  - SEP-NMPC was the only method that achieved 0% violation/infeasibility and 0% overshoot across 20 trials, demonstrating superior robustness and smoothness.

5. Significance

This work represents a significant advancement in UAV payload transportation by bridging the gap between theoretical stability guarantees and practical safety requirements in dynamic environments.

Practical Impact: It enables autonomous delivery and inspection missions in complex, unstructured environments (e.g., forests, urban canyons) where payload swinging is a major failure mode.
Theoretical Contribution: It demonstrates how to rigorously combine energy-based stability (Passivity) with set-theoretic safety (CBFs) within a single optimization loop, offering a blueprint for safe control of other underactuated, coupled systems.
Scalability: The QP-compatible nature of the solution suggests it can be deployed on resource-constrained edge devices, making it viable for commercial and industrial applications.