High-Speed Trajectory Tracking for Quadrotors with Aerodynamic Effects
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 asbaseline
.
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 beinglightweight
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
.
- The optimization is often intractable due to
- 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 thenominal quadrotor dynamics
, anaccurate 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.
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:
- \(\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
:
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.