Motivation

High-speed trajectory tracking

Accurate trajectory tracking with quadrotors in high-speed and high-acceleration regimes is still a challenging research problem.

  • Not only fly fast in known-free environments,
  • but also close to obstacles, humans, or through openings.
    • small deviations from the reference have catastrophic consequences.

The applications of quadrotor (such as agriculture, transport, security, etc) still do not exploit the full maneuverability of the quadrotors.

Very little work exists on agile control of quadrotors. Even through the works show agile control at various levels, none of them accounts for aerodynamic effects.

This is not a limiting assumption when the quadrotor is controlled closed to hover conditions, but introduces significant errors when tracking fast and agile trajectories.

Iterative learning control can be used to perform highly aggressive trajectories, but they are considered to a single maneuver and do not generalize.

Aerodynamic effects

Complex aerodynamic effects become a significant disturbance at high speeds:

  • introducing large positional tracking errors.
  • difficult to model.

Aerodynamic effects:

  • propeller lift and drag dependent on the induced airstream velocity.
  • fuselage drag.
  • turbulent effects due to the interaction between the propellers, the downwash of other propellers, and the fuselage.

Related work:

  • Identify a linear model for the rotor drag, integrate the model in an MPC pipeline to act as baseline.

Model of quadrotor

Model complexity is constrained by:

  • the feedback timescale.
  • computational capabilities of the executing platform.

It is not sufficient to find the most accurate model, but required to find an applicable trade-off between model accuracy and complexity.

Main challenge:

  • Identify a dynamics model of the quadrotor that is capable of describing the aerodynamic effects while still being lightweight enough to guarantee real-time performance.

Related work:

  • Most prior work on control of robots does not account for higher-order effects and treats them as external disturbances.
    • Allow for very efficient and lightweight controller implementations.
    • Tracking performance progressively decreases for higher speeds.
  • Learn the dynamics entirely from data.
    • Take the form of deep neural networks.
    • Substitute the nominal dynamics.
    • The learned dynamics are very expressive.
      • The optimization is often intractable due to local minima.
  • Combine a nominal model with a learned correction term.
    • Provides the possibility to learn only specific effects that are difficult to capture with the nominal model.

Contributions

  • Present an approach to model aerodynamic effects using Gaussian Processes.
    • Learn the aerodynamic effects acting on the quadrotor from data.
    • Use Gaussian Processes to learn the residual dynamics w.r.t. a simplified quadrotor model that does not account for aerodynamic effects.
    • By combining the learned GP corrections with the nominal quadrotor dynamics, an accurate dynamics model can be learned from a small number of inducing data points.
  • Incorporate into a MPC to achieve efficient and precise real-time feedback control.
  • \(70\%\) reduction in trajectory tracking error at high speeds.

Method

Nominal Quadrotor Dynamics Model

Control inputs: \(\mathbf{u} = [T_0,T_1,T_2,T_3]\).

\[\dot{\mathbf{x}} = \begin{bmatrix} \dot{\mathbf{p}} \\ \dot{\mathbf{q}} \\ \dot{\mathbf{v}} \\ \dot{\mathbf{\omega}}_b \end{bmatrix} = f_{dyn}(\mathbf{x},\mathbf{u}) = \begin{bmatrix} \mathbf{v} \\ \mathbf{q}\cdot\begin{bmatrix} 0 \\ \frac{\mathbf{\omega}_b}{2} \end{bmatrix} \\ \mathbf{q}\odot\mathbf{T}_b + \mathbf{g} \\ \mathbf{J}^{-1}(\mathbf{\tau}_b - \mathbf{\omega}_b\times\mathbf{J}\mathbf{\omega}_b) \end{bmatrix}.\]
  • \(\mathbf{g} = [0,0,-9.81m/s^2]^T\).
  • \(\mathbf{T}_b = \begin{bmatrix} 0 \\ 0 \\ \sum T_i \end{bmatrix}\).
  • \(\mathbf{\tau}_b = \begin{bmatrix} d_y(-T_0 - T_1 + T_2 + T_3) \\ d_x(-T_0 + T_1 + T_2 - T_3) \\ c_\tau(-T_0 + T_1 - T_2 - T_3) \end{bmatrix}\).

State update:

  • 4th order Runge-Kutta method.
\[\mathbf{x}_{k+1} = f_{RK4}(\mathbf{x}_k,\mathbf{u}_k,\Delta t).\]

Gaussian Process - Augmented Dynamics

Using GP to complement the nominal dynamics of the quadrotor in an MPC pipeline.

GPs predict the error of the dynamics and correct them at every time instance.

Assume the existence of the inaccessible true dynamics \(f_{true}\) of the quadrotor:

\[\tilde{\mathbf{x}}_{k+1} = f_{true}(\mathbf{x}_k,\mathbf{u}_k) + \mathbf{N}_k.\]
  • \(\mathbf{N}_k\sim N(\mathbf{0},\mathbf{\Sigma})\) is Gaussian noise, \(\mathbf{\Sigma}\) is the time-invariant and diagonal covariance matrix.
  • We can effectively treat each dimension of \(\mathbf{x}_k\) independently through a separate 1D output GP.

Define the Radial Basis Function (RBF) kernel as:

\[k(z_i,z_j) = \sigma_f^2 \exp\left( -\frac{1}{2} (z_i-z_j)^T \mathbf{L}^{-2}(z_i-z_j) \right) + \sigma_n^2,\]

where

  • \(\mathbf{L}\) is the diagonal length scale matrix.
  • \(\sigma_f,\sigma_n\) represent the data and prior noise variance, respectively.
  • \(z_i,z_j\) represent data features.

Redefine the dynamics as:

\[f_{cor}(\mathbf{x}_k,\mathbf{u}_k) = f_{dyn}(\mathbf{x}_k,\mathbf{u}_k) + \mathbf{B}_d \mu(z_k),\]

where

  • \(z_k = \mathbf{B}_z [\mathbf{x}_k^T\quad \mathbf{u}_k^T]^T\), \(\mathbf{B}_z\) is the feature selection matrix, \(z_k\) denotes the feature vector.
  • \(\mu\) is the mean of GP.
  • \(\mathbf{B}_d\) is the state selection matrix.
    • GP only corrects a subset of the state.

MPC Formulation

Stabilize the system subject to its dynamics \(\dot{\mathbf{x}} = f(\mathbf{x},\mathbf{u})\) along a reference \(\mathbf{x}^*(t),\mathbf{u}^*(t)\) by minimizing a cost function as:

\[\min_{\mathbf{u}} \int L(\mathbf{x},\mathbf{u}),\] \[\begin{aligned} s.t.\quad \dot{\mathbf{x}} & = f(\mathbf{x},\mathbf{u}), \\ \mathbf{x}(t_0) & = \mathbf{x}_{init}, \\ r(\mathbf{x},\mathbf{u}) & = 0, \\ h(\mathbf{x},\mathbf{u}) & \le 0. \end{aligned}\]

Define the cost to be of quadratic form:

\[L(\mathbf{x},\mathbf{u}) = \Vert \mathbf{x} - \mathbf{x}^*\Vert_Q^2 + \Vert \mathbf{u} - \mathbf{u}^*\Vert_R^2.\]

Then, the MPC can be written as:

\[\min_{\mathbf{u}} \mathbf{x}_N^T Q \mathbf{x}_N + \sum_{k=0}^N \mathbf{x}_k^T Q \mathbf{x}_k + \mathbf{u}_k^T R \mathbf{u}_k,\] \[\begin{aligned} s.t. \quad \mathbf{x}_{k+1} & = f_{RK4}(\mathbf{x}_k,\mathbf{u}_k,\Delta t), \\ \mathbf{x}_0 & = \mathbf{x}_{init}, \\ \mathbf{u}_{min} & \le \mathbf{u}_k \le \mathbf{u}_{max}. \end{aligned}\]

Where \(f_{RK4}\) can be extended to the corrected dynamics \(f_{cor}\).

Solve the quadratic optimization problem through a sequential quadratic program (SQP).

References

  • G. Torrente, E. Kaufmann, F. Philipp, and D. Scaramuzza, “Data-Driven MPC for Quadrotors,” pp. 1–8, 2020.
  • M. Faessler, A. Franchi, and D. Scaramuzza, “Differential Flatness of Quadrotor Dynamics Subject to Rotor Drag for Accurate Tracking of High-Speed Trajectories,” IEEE Robot. Autom. Lett., vol. 3, no. 2, pp. 620–626, Dec. 2018.