⚡ electrical engineering

Robust Global Position and Heading Tracking on SE(3) via Saturated Hybrid Feedback

This paper proposes a robust hybrid feedback control architecture for underactuated vehicles on SE(3) that achieves global position and heading tracking with user-defined actuation limits by combining a saturated position controller with a saturated MRP-based attitude controller enhanced by a hybrid path-lifting mechanism.

Original authors: Luís Martins, Carlos Cardeira, Paulo Oliveira

Published 2026-03-20

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Luís Martins, Carlos Cardeira, Paulo Oliveira

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to teach a very clumsy, heavy drone to fly a perfect figure-eight pattern in a windy park. But there's a catch: the drone's motors are weak (it can't push too hard), its propellers can only spin so fast (they have a speed limit), and it can't twist its body too violently without breaking its frame.

This paper presents a new "brain" (a control algorithm) for such a drone. It solves the problem of how to make this weak, limited drone fly exactly where you want it to go, while keeping its orientation perfect, even if it starts upside down or in a terrible position.

Here is the breakdown of their solution using everyday analogies:

1. The Problem: The "Clumsy Dancer"

Most drones are underactuated. Think of a dancer who can only push forward with their feet but has no arms to balance. If they want to turn left, they have to lean (tilt) their whole body.

The Issue: If the dancer leans too far, they might fall. If they try to move too fast, their muscles (motors) hit a limit and stop working (saturation).
The Topology Trap: The paper mentions a math problem called "SE(3)." Imagine trying to draw a perfect circle on a globe without lifting your pen. If you try to do it with a simple, smooth line, you eventually get stuck or have to jump. In math terms, you can't smoothly stabilize a 3D object from every starting point without a "jump" or a special trick.

2. The Solution: A Two-Part Team

The authors built a control system with two main parts working together: an Outer Loop (the Navigator) and an Inner Loop (the Pilot).

Part A: The Navigator (Outer Loop) – "The Filtered GPS"

The Navigator's job is to say, "We need to go there."

The Old Way: Previous methods tried to calculate the exact path instantly. If the drone was too far off, the math would scream "GO FAST!" which would break the weak motors.
The New Trick (The Filters): The authors added two "filters" (think of them as shock absorbers or sponges).
- Instead of the Navigator screaming "GO NOW!", the signal passes through a sponge. It smooths out the sudden jumps.
- The Magic: They used a clever mathematical trick (a coordinate transformation) to pretend the sponge doesn't exist when designing the plan. This allows them to prove mathematically that the drone will never ask for more power than it has, even if the path is crazy. It guarantees the drone stays within its "muscle limits."

Part B: The Pilot (Inner Loop) – "The Hybrid Gymnast"

Once the Navigator says "Tilt 45 degrees to the left," the Pilot has to actually move the drone's body to do it.

The Problem: If the drone is upside down, a simple "tilt left" command might make it spin the wrong way (like the shortest path on a globe vs. the long way around).
The New Trick (Hybrid Path-Lifting): The Pilot uses a "Hybrid" strategy. Imagine a gymnast doing a flip. Sometimes, to get to the right pose, they have to spin the long way around rather than the short way, just to avoid getting stuck.
- The system uses a "covering space" (a mathematical map that wraps around the world multiple times).
- If the drone gets confused or starts spinning the wrong way, the system detects it and performs a "jump" (a quick switch in logic) to reset the orientation, ensuring the drone always finds the most efficient way to the target without getting stuck in a loop.

3. The "Hybrid" Magic

The word "Hybrid" in the title is key. It means the system uses both smooth driving (when things are going well) and sudden jumps (when the drone is confused or upside down).

Analogy: Think of driving a car. Usually, you steer smoothly. But if you hit a patch of ice and start spinning, you might slam the brakes or jerk the wheel (a discrete jump) to regain control. This paper builds a system that knows exactly when to "jerk the wheel" mathematically to prevent the drone from crashing.

4. Why This Matters

Safety: It guarantees the drone will never ask its motors to do the impossible. It respects the "speed limit" of the hardware.
Robustness: Even if there is wind, if the drone starts upside down, or if the sensors are a little noisy, this system recovers and gets the job done.
Global Stability: It works from any starting position. You don't have to carefully place the drone on the ground before turning it on; you can drop it from a tree, and this brain will catch it and fly it home.

Summary

The authors created a smart, safety-conscious brain for weak drones.

It uses sponges (filters) to smooth out commands so the drone doesn't break its motors.
It uses a gymnast's logic (hybrid jumps) to untangle itself if it gets upside down or confused.
It mathematically proves that no matter how messy the start is, the drone will eventually fly the perfect path without ever exceeding its physical limits.

It's like giving a clumsy, weak robot a coach who knows exactly how to push it without breaking it, and who knows how to reset its brain if it gets dizzy.

1. Problem Statement

The paper addresses the robust global position and heading tracking problem for underactuated vehicles (e.g., quadrotors, drones) operating under strict, user-defined actuation limits.

System Dynamics: The vehicles are modeled as rigid bodies with single-axis thrust (force along the body $z$ -axis) and full torque actuation. The dynamics evolve on the Special Euclidean group $SE(3)$.
Core Challenges:
1. Underactuation: The number of degrees of freedom exceeds the control inputs, creating a structural deficit where thrust direction dictates attitude.
2. Actuator Saturation: Physical limits on thrust ( $T \le T_{max}$ ) and torque ( $\tau \le \tau_{max}$ ) are unavoidable. Ignoring these leads to performance degradation or instability.
3. Topological Obstructions: The configuration space $SE(3)$ (specifically the rotation group $SO(3)$) is not contractible. This prevents global stabilization via continuous feedback alone. Discontinuous controllers, while capable of global stability, lack robustness against small disturbances (chattering or trapping in undesired regions).
4. Singularity Avoidance: Standard hierarchical control structures often fail when commanded thrust vanishes, losing the ability to define a valid attitude reference.
5. Coupling: Existing solutions often handle thrust saturation but neglect torque saturation, or restrict heading tracking to specific conditions (e.g., zero roll/pitch).

Objective: Design bounded control inputs ( $T, \tau$ ) such that the vehicle achieves robust global asymptotic and semi-global exponential tracking of a reference trajectory (position and heading) while strictly respecting input saturation bounds.

2. Methodology

The proposed solution is a hierarchical hybrid control architecture consisting of an outer position loop and an inner attitude loop, interconnected via a stability equivalence framework.

A. Outer Loop: Saturated Position Tracking with Filters

Instead of using nested saturation functions (which complicate derivative bounds), the authors introduce a saturated position controller augmented with two first-order filters.

Structure: The control law $u$ (virtual control force) is split into a feedforward term and a filtered feedback term $u_f$ .
Filter Dynamics: Two cascaded first-order filters ( $\dot{u}_f, \dot{u}_s$ ) are applied to the primary controller output.
Coordinate Transformation: A key innovation is a forward-projected coordinate transformation ( $z = \Phi_p \tilde{x}_p$ $z = Φ_{p} \tilde{x}_{p}$ ). This transformation algebraically decouples the filter dynamics from the tracking errors.
- It maps the physical system with finite bandwidth filters into a "transformed" system that behaves like an ideal double integrator.
- This allows the controller to be designed assuming infinite bandwidth, while the transformation mathematically accounts for the phase lag introduced by the filters.
Benefits: This approach yields analytically tractable, non-conservative bounds for the control signal and its first two derivatives. These bounds are easily tunable via filter gains ( $k_f, k_s$ ), facilitating the verification of trajectory feasibility before implementation.

B. Inner Loop: Saturated Hybrid Attitude Tracking

The attitude controller tracks the dynamic references generated by the outer loop.

Representation: Uses Modified Rodrigues Parameters (MRP) to parameterize the attitude error. MRPs avoid singularities within a specific region and allow for a "shadow" set to handle the double-covering of $SO(3)$.
Hybrid Path-Lifting: To achieve global stability on $SO(3)$, the controller employs a hybrid dynamic path-lifting mechanism.
- It switches between the principal and shadow MRP representations based on a hysteresis logic.
- This avoids the topological obstruction of $SO(3)$, ensuring the system does not get trapped in local minima.
Control Law: A saturated feedback law based on MRP errors, paired with a feedforward term to cancel coupling dynamics. The saturation functions ensure torque limits are respected.

C. Interconnection and Stability Analysis

Stability Equivalence: The authors leverage a stability equivalence framework to prove that the stability of the interconnected system (position + attitude) holds on $SE(3)$.
Lyapunov Analysis:
- Outer Loop: Proves global asymptotic and semi-global exponential stability using a composite Lyapunov function that accounts for the filter states.
- Inner Loop: Proves robust global asymptotic and semi-global exponential stability for the hybrid attitude system.
- Interconnected System: Uses $L_2$ -stability arguments and small-gain-like reasoning (via exponential decay rates) to show that the coupling term (perturbation from attitude error to position dynamics) is square-integrable and decays exponentially, ensuring the overall system stability.

3. Key Contributions

Novel Filter-Augmented Architecture: Replaces complex nested saturation functions with a filter-based approach that simplifies the derivation of derivative bounds and allows for systematic tuning of reference limits.
Forward-Projected Coordinate Transformation: A geometric decoupling technique that structurally isolates filter dynamics, enabling the design of controllers as if actuation were ideal, while rigorously proving stability under real constraints.
Simultaneous Thrust and Torque Saturation: Unlike prior works that often ignore torque limits or restrict heading tracking, this solution enforces strict bounds on both thrust and torque inputs simultaneously.
Unrestricted Heading Tracking: The formulation ensures exact yaw tracking is possible without restricting roll and pitch to zero, overcoming geometric coupling limitations found in previous literature (e.g., [9]).
Rigorous Stability Guarantees: Establishes robust global asymptotic stability and semi-global exponential stability for the full $SE(3)$ system under input saturation, a first for underactuated thrust-propelled vehicles with simultaneous thrust and torque limits.

4. Simulation Results

Numerical simulations were conducted on a rigid-body model with:

Mass: 0.46 kg, Inertia: Diagonal matrix.
Limits: $T_{max} = 7$ N, $\tau_{max} = (0.5, 0.5, 0.5)$ Nm.
Scenario: The vehicle started in a challenging downward-facing attitude (Euler angles $\approx -179^\circ$ roll) with large initial position errors, tasked with tracking a sinusoidal 3D trajectory while rotating its heading.

Key Findings:

Recovery: The system successfully recovered from the extreme initial attitude and large position errors.
Tracking Performance: Position and MRP error norms converged to steady-state values below $0.001$ m and $0.0001$, respectively.
Saturation Compliance: The generated thrust and torque signals strictly respected the theoretical bounds derived in the theorems.
Robustness: The controller maintained performance despite the transient lag introduced by the filters and the initial saturation of control signals.

5. Significance

This work bridges a critical gap in nonlinear control theory for autonomous vehicles. By providing a globally stable, robust, and exponentially convergent solution that explicitly handles both thrust and torque saturation, it enables safer and more reliable operation of underactuated vehicles in constrained environments. The use of hybrid feedback overcomes topological limitations, while the filter-augmented design offers a practical, tunable framework for real-world implementation where actuator limits are strict and non-negotiable. The methodology sets a new standard for high-performance trajectory tracking on $SE(3)$.