High-level spatial Dubins airplane-based reference smoothing with low-level geometric tracking for quadrotor control

Problem Formulation

The paper addresses the challenge of controlling quadrotors to track "edgy" reference paths—defined as sparse sets of waypoints with large inter-waypoint spacing and abrupt heading changes (e.g., 90° turns)—while satisfying lateral constraints (such as avoiding obstacles on one side of the path).

The authors identify a gap in existing literature where methods often either:

1. Require complex, high-dimensional nonlinear model predictive control (NMPC) with difficult hyperparameter tuning and short prediction horizons.
2. Use spline-based smoothing (e.g., B-splines, Bezier curves) that may fail to closely track sparse waypoints or lack suitability for specific applications.
3. Rely on low-level controllers directly tracking edgy references, which can lead to "corner-cutting" and violation of lateral safety constraints.

The goal is to develop a method that hierarchically reduces complexity from planning to tracking, maintains trajectory shaping capabilities along the entire path (not just at control points), and minimizes the number of hyperparameters.

Methodology

The proposed solution is a two-layered approach combining high-level reference smoothing with low-level geometric tracking.

1. High-Level Reference Smoothing (HLRS)

This step generates a smooth, feasible reference trajectory from the initial edgy waypoints.

• Model: It utilizes a Dubins airplane model with four states: position $(x, y, z)$ and yaw angle $\psi$. This model allows for dimensionality reduction and decoupling of the $z$-dynamics.
• Optimization: The problem is formulated as a Linear Program (LP) using a spatial modeling approach. The objective is to minimize lateral error subject to constraints:
  • Lateral constraints (e.g., staying to the right of the path to avoid obstacles).
  • Roll angle limits ($\phi_{min} \leq \phi_k \leq \phi_{max}$).
  • The LP is small-scale; for a 10m horizon with 1m spacing, it involves only ~21 optimization variables.
• Filtering: A post-processing filtering step (Savitzky-Golay or Exponential Moving Average) is applied to the LP output to ensure algebraic smoothness required by the low-level controller.
• Velocity Profiling: A velocity profile is assigned based on:
  • Cruise velocity.
  • A roll-angle-based speed bound derived from the Dubins model ($v_{roll}^{max}$).
  • A braking profile derived from Newton's second law to ensure the quadrotor can stop at the goal position ($v_{brake}^{max}$).
  • The final reference velocity is $v_{ref}(s) = \min(v_{cruise}, v_{roll}^{max}, v_{brake}^{max})$. This allows for non-uniform spatial interpolation, which is crucial for the low-level controller's feedforward terms.

2. Low-Level Reference Tracking

This step executes the tracking of the smoothed reference using the full quadrotor dynamics.

• Model: The full 12-state quadrotor model (position, velocity, Euler angles, and angular rates) is used.
• Controller: A geometric tracking controller (based on [14]) is employed. This controller avoids singularities associated with Euler angles and ambiguities of quaternions.
• Implementation: The controller relies heavily on feedforward terms derived from the reference trajectory's position, velocity, acceleration, and yaw. The non-uniform spatial grid generated in the high-level step is critical for extracting these derivatives accurately.
• Tuning: The controller uses four diagonal tuning matrices ($K_p, K_v, K_R, K_\Omega$). The paper notes that these were selected based on orders of magnitude (powers of ten) without extensive fine-tuning.

3. Operational Modes (M1 and M2)

The paper defines two variations of the method:

• Method M1 (Offline Planning): The high-level smoothing is performed once offline over the entire path. The resulting smooth reference is then tracked online. This is suitable for static environments (e.g., agricultural area coverage).
• Method M2 (Online Receding Planning): The high-level smoothing is performed online in a receding horizon fashion over a limited spatial prediction horizon $H$. At each step, the current position is connected to the original edgy path, smoothed, and then tracked. This is suitable for dynamic environments or changing waypoints.

Key Contributions

1. Hierarchical Complexity Reduction: The method decouples the problem, using a low-dimensional Dubins model for planning and a full model for tracking, significantly reducing computational load compared to full NMPC.
2. Spatial LP Formulation: By leveraging spatial modeling and the Dubins structure, the planning step is reduced to a small Linear Program with a hyperparameter-free objective function (minimizing lateral error).
3. Lateral Constraint Handling: The approach explicitly enforces lateral constraints (e.g., obstacle avoidance on one side) throughout the trajectory, preventing corner-cutting.
4. Velocity-Profile Integration: The generation of a non-uniform spatial grid based on dynamic braking and roll limits ensures the low-level controller receives a reference compatible with its feedforward requirements.

Numerical Results

Three numerical experiments were conducted on a laptop (Intel Core i9) comparing M1 and M2 against a cascaded PID controller and different smoothing filters.

• Performance: The proposed geometric controller significantly outperformed the cascaded PID approach. In Example 1 (M1), the geometric controller achieved an RMSE of 0.174 m (with SG-filter), whereas the PID controller resulted in RMSEs of 0.324 m and 0.356 m.
• Filtering: The Savitzky-Golay (SG) filter provided slightly better accuracy (9% lower RMSE) than the Exponential Moving Average (EMA) filter, though EMA is algebraically simpler.
• M1 vs. M2:
  • M1 provided consistent tracking over large horizons with low computational cost (LP solve time ~0.007–0.017s).
  • M2 demonstrated effective online planning. A larger prediction horizon ($H=20$m vs $10$m) improved tracking performance (more anticipative maneuvers) but increased solve time (0.008s vs 0.003s).
  • Overshooting: M2 exhibited overshooting behavior due to the limited horizon. The authors proposed a mitigation strategy: replanning only every second sampling time while using the shifted previous plan in between, which reduced computational load without sacrificing stability.

Significance and Claims

The paper claims that the proposed two-level approach successfully:

• Smooths edgy reference paths over large planning horizons at low computational cost.
• Maintains reference tracking accuracy and lateral trajectory shaping capabilities.
• Reduces the number of hyperparameters to a minimum (the LP objective is hyperparameter-free; controller tuning is limited to magnitude selection).
• Offers a viable alternative to complex NMPC or spline-based methods, particularly for applications requiring strict lateral constraints (like obstacle contour tracking).

The authors acknowledge limitations, noting that the method relies on the Dubins model for planning (which is an approximation of the full quadrotor dynamics) and that future work could explore using the full 12-state model for the optimization step, though this would introduce significant complexity and initialization challenges.