\documentclass[a4paper, 10pt]{IEEEtran}

\usepackage{array}
\usepackage{booktabs}
\usepackage{siunitx}
\usepackage{float}

\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{oststud}

\usepackage{graphicx}
\usepackage{subcaption}
\usepackage{pgfplots}
\pgfplotsset{compat=1.18}
\usepackage{tikz}
\usetikzlibrary{3d,perspective,matrix,calc}

% Bibliography IEEE style
\usepackage[
  backend=biber,
  style=ieee,
  citestyle=numeric-comp,
  sortcites=true
]{biblatex}

% Allow breaking long URLs in the bibliography
\setcounter{biburllcpenalty}{7000}
\setcounter{biburlucpenalty}{8000}

% Add a bibliography resource
\addbibresource{RCCO.bib}

\title{
  Robust $\mathcal{H}_\infty$ Controller Synthesis for Hovering Ducted-Fan VTOL
  Micro-UAV
}
\author{
  \IEEEauthorblockN{Naoki Sean Pross} \IEEEauthorrefmark{1}
  % \thanks{
  % }
  \thanks{
    Midterm project for course \emph{Robust Control and Convex Optimization}
    taught by Prof. Dr. R. S. Smith at ETH Zurich.
    \IEEEauthorrefmark{1}Student ID:~\texttt{19-150-903}
    Email:~\texttt{npross@student.ethz.ch}.
    Thanks to T. Rothlin for providing the electromechanical parameters for the
    UAV.
  }
}

\begin{document}

\maketitle

\begin{abstract}
  This paper models an experimental vertical take-off landing (VTOL) micro
  unmanned aerial vehicle (UAV) device developed at the University of Applied
  Sciences OST RJ in Rapperswil. The model is then converted to a linear time
  invariant (LTI) state space plant to design a robust controller using
  $\mathcal{H}_\infty$ synthesis that tracks a piecewise constant spatial
  position reference. In conclusion simulated preliminary results on the
  controller performance are presented.
\end{abstract}

\section{Introduction}

The presented VTOL micro UAV is a device developed by T. Rothlin
\cite{rothlin_optimization_2024}  at the University of Applied Sciences OST RJ,
Rapperswil that can be steered only by controlling the propeller velocity and
the angle of attack of four flaps under the ducted fan. Currently there is not
a control algorithm for the UAV and the uncertainties from aerodynamics and
gyroscopic effects caused by the propeller make this an interesting candidate
for robust controller synthesis.

% \subsection{Notation}
% 
% Throughout this text we will make use a notation customary to mechanics, using
% boldface letters for vectors and matrices and a hat to indicate unit vectors.
% Spatial coordinates are grouped into a position vector $\vec{P}$ to avoid
% confusing the $x$ and $y$ coordinates with the state space model's output and
% state vectors $\vec{y}$ and $\vec{x}$.

% \begin{table}
%   \centering
%   \caption{
%     Summary of notation for physical quantities.
%     \label{tab:sym-phy}
%   }
%   \begin{tabular}{cl}
%     \toprule
%     Symbol & Physical Quantity \\
%     \midrule
%     $\vec{P}$ & Position vector \\
%     $\vec{F}$ & Total force \\
%     $\vec{M}$ & Total torque \\
%     \bottomrule
%   \end{tabular}
% \end{table}

% \subsection{Organization}
% 
% Section \ref{sec:modelling} presents a derivation of a model for the plant, and
% then \S\ref{sec:control} illustrates the controller design. Finally, in
% \S\ref{sec:simulation} the closed loop design is simulated and briefly
% discussed.

\section{System Modelling}
\label{sec:modelling}

% \subsection{Reference Frames}

To model the dynamics of the ducted-fan UAV two reference frames are required:
an inertial frame and a body-frame attached to the center of mass
\cite{stengel_flight_2022}. In the inertial frame we work in the base given by
the unit vectors $\{\uvec{\imath},\uvec{\jmath},\uvec{k}\}$, whereas in the
body-frame we use $\{\uvec{x}, \uvec{y}, \uvec{z}\}$ together with the Euler
angles $\vec{\Theta} = [ \phi ~ \vartheta ~ \psi ]^\mathsf{T}$. The angular
velocity $\vec{\Omega} = [p ~ q ~ r]^\mathsf{T}$ is then related to
$\vec{\Theta}$ by $\dot{\vec{\Theta}} = \mx{U} \vec{\Omega}$. To rotate from
the inertial frame to body frame we use the $SO(3)$ matrix $\mx{R}$. Both
$\mx{U}$ and $\mx{R}$ can be found in \cite{stengel_flight_2022}.

% \begin{equation}
%   \dot{\vec{\Theta}} = \begin{bmatrix}
%     1 & S_\phi T_\vartheta & C_\phi T_\vartheta \\
%     0 & C_\phi & - S_\phi \\
%     0 & S_\phi / C_\vartheta & C_\phi / C_\vartheta
%   \end{bmatrix}
%   \vec{\Omega}
%   = \mx{u} \vec{\omega},
% \end{equation}
% wherein we use $S_\alpha$, $C_\alpha$ and $T_\alpha$ as shorthand for $\sin
% \alpha$, $\cos \alpha$ and $\tan \alpha$ respectively. To rotate from the
% inertial frame to body frame we use the $SO(3)$ matrix
% \begin{equation*}
%   \mx{R} = \begin{bmatrix}
%     C_\vartheta C_\psi & C_\vartheta S_\psi & - S_\vartheta \\
%     S_\phi S_\vartheta C_\psi - C_\phi S_\psi 
%       & S_\phi S_\vartheta C_\psi + C_\phi C_\psi
%       & S_\phi C_\vartheta \\
%     C_\phi S_\theta C_\psi + S_\phi S_\psi
%       & C_\phi S_\theta S_\psi - S_\phi C_\psi
%       & C_\phi C_\vartheta
%   \end{bmatrix}.
% \end{equation*}

\subsection{Equations of Motion}

Consider a simplified rigid-body physical model sketched in
Fig.~\ref{fig:phy-sketch}. With respect to the inertial frame the equations of
motion from the Newton-Euler formalism are
\begin{subequations}
  \begin{align}
    m \ddot{\vec{P}} &= \mt{\mx{R}} \vec{F} \\
    \vec{J} \dot{\vec{\Omega}} &= 
      - \vec{\Omega} \crossp \vec{J} \vec{\Omega} + \vec{\tau},
  \end{align}
\end{subequations}
where $m$ is the total mass, $\vec{P}$ is the position, $\vec{F}$ and
$\vec{\tau}$ the total force and torque in the body frame respectively, and
finally $\mx{J}$ the moment of inertia, which is assumed to be a diagonal
matrix.

\begin{figure}
  \centering
  % \includegraphics[width=.7\linewidth, trim={0 10 0 0}, clip]{fig/sketch_hand}
  \input{fig/sketch}
  \caption{
    Sketch of the rigid body physical model for the UAV.
    % \texttt{TODO: replace with computer-drawn sketch.}
    \label{fig:phy-sketch}
  }
\end{figure}

\begin{figure*}[t]
  \centering
  \begin{tikzpicture}[font = \footnotesize]
    \begin{axis}[
        width = 42mm, height = 28mm,
        scale only axis,
        grid = major,
        grid style = {densely dotted, gray!50},
        ylabel = {Flap Angle $\alpha$ / degrees},
        xlabel = {Time $t$ / seconds},
        legend entries = {
          $\alpha_1(t)$,
          $\alpha_2(t)$,
          $\alpha_3(t)$,
          $\alpha_4(t)$
        },
        legend columns = 2,
        title = {\scshape Step Response of Flap Angles},
        /tikz/line join=bevel,
      ]
      \addplot+ [mark=none] table [x index = 0, y index = 7] {fig/stepsim.dat};
      \addplot+ [mark=none] table [x index = 0, y index = 8] {fig/stepsim.dat};
      \addplot+ [mark=none] table [x index = 0, y index = 9] {fig/stepsim.dat};
      \addplot+ [mark=none] table [x index = 0, y index = 10] {fig/stepsim.dat};
    \end{axis}
  \end{tikzpicture}
  \hfill
  \begin{tikzpicture}[font = \footnotesize, ]
    \begin{axis}[
        width = 42mm, height = 28mm,
        scale only axis,
        grid = major,
        grid style = {densely dotted, gray!50},
        ylabel = {Euler Angle / degrees},
        xlabel = {Time $t$ / seconds},
        legend entries = {
          $\phi(t)$,
          $\vartheta(t)$,
          $\psi(t)$,
        },
        legend columns = 2,
        legend style = {anchor = south east, at = {(.95,.05)}},
        title = {\scshape Step Response of Euler Angles},
      ]
      \addplot+ [thick, mark=none] table [x index = 0, y index = 11] {fig/stepsim.dat};
      \addplot+ [thick, mark=none] table [x index = 0, y index = 12] {fig/stepsim.dat};
      \addplot+ [thick, mark=none] table [x index = 0, y index = 13] {fig/stepsim.dat};
    \end{axis}
  \end{tikzpicture}
  \hfill
  \begin{tikzpicture}[font = \footnotesize]
    \pgfplotsset{set layers}
    \begin{axis}[
        width = 42mm, height = 28mm,
        scale only axis,
        grid = major,
        grid style = {densely dotted, gray!50},
        axis y line* = left,
        ylabel = {Coordinate / meters},
        xlabel = {Time $t$ / seconds},
        legend entries = {
          $x(t)$,
          $y(t)$
        },
        title = {\scshape Step Response of Position},
      ]
      \addplot+ [thick, mark=none] table [x index = 0, y index = 1] {fig/stepsim.dat};
      \addplot+ [thick, mark=none] table [x index = 0, y index = 2] {fig/stepsim.dat};
    \end{axis}
    \begin{axis}[
        width = 42mm, height = 28mm,
        scale only axis,
        % grid = major,
        % grid style = {densely dotted, gray!50},
        axis y line* = right,
        axis x line = none,
        ylabel = {Velocity / \unit{\meter\per\second}},
      ]
      \addplot+ [thick, mark=none, densely dotted, opacity=.8]
        table [x index = 0, y index = 4] {fig/stepsim.dat};
      \addplot+ [thick, mark=none, densely dotted, opacity=.8]
        table [x index = 0, y index = 5] {fig/stepsim.dat};
    \end{axis}
  \end{tikzpicture}
  \caption{
    Simulated step responses along $x$ of the closed loop $\mathcal{H}_\infty$
    controller design. In the third plot from the left, continuous lines show
    the position error while dotted lines with the right axis are the velocity
    in the corresponding directions.
    \label{fig:step}
  }
\end{figure*}

In the body frame we model the total force $\vec{F}$ acting on the UAV
following \cite{naldi_design_2010} by considering a thrust force from the
ducted-fan $\vec{F}_T = - k_T \omega^2 \uvec{z}$ as a function of the
propeller's angular velocity $\omega$. Because of the geometry we approximate
the generated air velocity field in the duct  as being constant and collinear
to $\uvec{z}$  \cite{pereira_hover_2008}
\begin{equation}
  \vec{\nu} = \sqrt{\frac{F_T}{2a\varrho \pi^2}} \uvec{z}
  = \frac{\omega}{\pi} \sqrt{\frac{k_T}{2a\varrho}} \uvec{z}.
\end{equation}
The \emph{drag} and \emph{lift} forces generated by the interaction of each
flap with the air velocity field are then $\vec{F}_d = \frac{1}{2} \varrho S
C_d\nu^2 \uvec{z}$ and $\vec{F}_\ell = \frac{1}{2} \varrho S C_\ell \nu^2
\uvec{n}$ respectively ($\uvec{n}$ being given by the orientation of the flap).
The density of air $\varrho$ is assumed to be constant, while the drag
coefficients $S$, $C_d$ and $C_\ell$ depend on the angle of attack $\alpha$ of
the flap making them quite difficult to determine. Hereinafter, under the
assumption that $\alpha$ is small they will be approximated with $C_d = c_d
\alpha^2 + c_{0}$, $C_\ell = c_\ell \alpha$ \cite{stengel_flight_2022,
pereira_hover_2008} and $S$ is considered a constant. Finally, gravity adds a
term $\vec{F}_g = mg \mt{\mx{R}} \uvec{k}$.

For the total torque it is derived from the geometry that each flap induces
$\vec{\tau}_{f} = (\frac{1}{3}a \uvec{t} + d\uvec{z}) \crossp (\vec{F}_d +
\vec{F}_\ell)$, with $\uvec{t} = \uvec{n} \crossp \uvec{z}$. In addition there
is also a torque induced by the gyroscopic procession $\vec{\tau}_g =
\mx{R}^\mathsf{T} \omega J_r \uvec{k} \crossp \vec{\Omega}$ with $J_r$ being
the inertia of the propeller with respect to its spin axis
\cite{naldi_design_2010}. The resulting total quantities are then
\begin{subequations}
  \begin{align}
    \vec{F} &= mg \mt{\mx{R}} \uvec{k}
      -k_T\omega^2 \uvec{z} \nonumber \\
      &\quad + \frac{\varrho S \nu^2}{2} \sum_i 
        \left(c_d \alpha^2_i + c_0 \right) \uvec{z}
        + c_\ell \alpha_i \uvec{n}_i, \\
    \vec{\tau} &= \omega J_r \mx{R}^\mathsf{T} (\uvec{k} \crossp \vec{\Omega})
      + \frac{\varrho c_\ell S \nu^2 d}{2} 
      \uvec{z} \crossp \sum_i \alpha_i \uvec{n}_i.
  \end{align}
\end{subequations}

% \subsection{Wind Gusts Model}
% 
% \texttt{TODO: add disturbance from wind modelled as simple, slowly changing
% stochastic model following empirical approach as done in
% \cite{chirarattananon_dynamics_2017}. Use 3D state with azimuth $\gamma$,
% zenith $\beta$ and magnitude $W$ to generate force and torque. Maybe
% make cyclostationary?}


\subsection{Linearized Dynamics and Plant Model}

To synthesize a controller we simplify the dynamics to a state-space LTI model
$\dot{\vec{x}} = \mx{A}_x \vec{x} + \mx{B}_{xu} \vec{u}$, $\vec{y} =
\mx{C}_{yx} \vec{x} + \mx{D}_{yu}\vec{u}$ with state and inputs given by
\begin{equation}
  \vec{x} = \mt{\begin{bmatrix}
    \mt{\vec{P}} &
    \mt{\dot{\vec{P}}} &
    \mt{\vec{\Theta}} & 
    \mt{\vec{\Omega}}
  \end{bmatrix}},
  \quad
  \vec{u} = \mt{\begin{bmatrix}
    \mt{\vec{\alpha}} & \omega
  \end{bmatrix}}
\end{equation}
respectively. The linearization is performed in a stationary hovering state at
height $h$ above the ground and yaw angle of \ang{45} or $\vec{P}_0 =
-h\uvec{k}$, $\vec{\Theta}_0 = \frac{\pi}{4} \uvec{\psi}$, thus $x_0 =
\mt{[\mt{\vec{P}}_0 ~ \mt{\vec{0}} ~ \mt{\vec{\Theta}}_0 ~ \mt{\vec{0}}]}$ and
$u_0 = \mt{[ \mt{\vec{0}} ~ \omega_0 ]}$, where $\omega_0 \approx \sqrt{mg /
k_T}$ is the propeller's angular velocity to make the UAV hover.

% \subsection{Plant Model}

The state is assumed to be known, as there is an inertial measurement unit with
a dedicated sensor fusion chip onboard \cite{rothlin_optimization_2024}. Thus,
we only need to model a measurement delay $T_m$ and the actuators. The
linearized dynamics are extended with 2\textsuperscript{nd} order Padé
approximant $G_{m}(s) \approx e^{-sT_m}$ for the output delay and two transfer
functions $G_\alpha(s)$, $G_\omega (s)$ for the flaps and thruster
respectively. Specifically, $G_\alpha(s)$ is a 2\textsuperscript{nd} order low
pass filter (LPF) with a slight overshoot, while $G_\omega(s)$ is a first order
LPF. All inputs and outputs are then normalized and a source of additive
Gaussian noise $\vec{w}_\alpha \sim \mathcal{N}(\vec{0}, \mx{I})$ is introduced
to model the wiggling in the servo motor of each flap. The resulting
plant has 4 unobservable and 4 uncontrollable modes, however it remains both
detectable and stabilizable.

\section{Controller Design}
\label{sec:control}

% \subsection{Control Problem}

The control objective is to track a piecewise constant position reference
$\vec{r}(t) \in \mathbb{R}^3$ in the inertial frame. The controller should move
the UAV to $\vec{r}(t)$ with control actions that do not exceed the actuator
limits $\alpha \in (\underline{\alpha}, \overline{\alpha})$, $\omega \in [0,
\overline{\omega})$, do not bring the angular components of the state too far
from their set point, i.e. $\vec{\Theta} \in (-\varphi,\varphi)^3$, and do not
exceed the velocity limits $\|\dot{\vec{P}}\|_1 \leq \overline{v}$.

% \subsection{$\mathcal{H}_\infty$ Synthesis}

To perform $\mathcal{H}_\infty$ synthesis the system is first rewritten in the
form presented in \cite{skogestad_multivariable_2010}, in terms of control
inputs $\vec{u} \in \mathbb{R}^5$, exogenous inputs $\vec{w} =
\mt{[\mt{\vec{w}}_\alpha ~ \mt{\vec{r}}]} \in \mathbb{R}^7$, control outputs
$\vec{y}_m = \mt{[\mt{(\vec{r} - \vec{y})} ~ \mt{\dot{\vec{P}}} ~
\mt{\vec{\Theta}} ~ \mt{\vec{\Omega}}]}$ and errors $\vec{e} =
\mt{[\mt{\vec{\alpha}}~\omega~\mt{(\vec{r} - \vec{y})} ~ \mt{\dot{\vec{P}}} ~
\mt{\vec{\Theta}}]}$ and partitioned accordingly:
\begin{equation}
  \begin{bmatrix} \vec{e} \\ \vec{y}_m \end{bmatrix}
    = \begin{bmatrix}
      \mx{A}_{ew} & \mx{B}_{eu} \\
      \mx{C}_{yw} & \mx{D}_{yu}
    \end{bmatrix}
  \begin{bmatrix} \vec{w} \\ \vec{u} \end{bmatrix}
    = \mx{G}_\text{nom}
  \begin{bmatrix} \vec{w} \\ \vec{u} \end{bmatrix}.
\end{equation}

Then for each component of the error a performance weight function is defined
resulting in 5 transfer functions $W_{P,\alpha}(s)$, $W_{P,\omega}(s)$,
$W_{P,\vec{P}}(s)$, $W_{P,\dot{\vec{P}}}(s)$, $W_{P,\vec{\Theta}}(s)$. The
$\mathcal{H}_\infty$ synthesis is performed using the Riccati method yielding a
stable controller $\mx{K}_\infty$ with $\gamma =
\|\mathcal{L}(\mx{G}_\text{nom}, \mx{K}_\infty)\|_\infty \approx 0.9091$. Here
we used the notation $\mathcal{L}$ for the linear fractional transformation
interconnection.

\section{Preliminary Results} \label{sec:simulation}

The resulting closed loop plant $\mx{G}_\text{cl} =
\mathcal{L}(\mx{G}_\text{nom}, \mx{K}_\infty)$ is simulated with a step of
\qty{50}{\cm} along the $x$ spatial coordinate and shown in
Fig.~\ref{fig:step}. Currently, it takes the UAV about 30 seconds to track a
half meter step, while respecting the constraints on the actuators. Although
stable and with good noise rejection characteristics the current design does
not take into account large inaccuracies caused by the highly non linear
equations of motion. For higher speeds necessitate higher pitch and roll
angles, a more performant controller will need to cope with higher modelling
errors. Hence the next controller will be designed with $\mu$-synthesis which
can take into account the aforementioned modelling uncertainties.

\printbibliography

\iffalse
\clearpage
\appendix

\subsection{Linearized Model Details}

\begin{equation*}
  A = \begin{bmatrix}
    0 & I & 0 & 0 \\
    0
      & \frac{1}{m} \mt{\mx{R}} \frac{\partial \vec{F}}{\partial \dot{\vec{P}}}
      & \frac{1}{m} \frac{\partial (\mt{\mx{R}} \vec{F})}{\partial \vec{\Theta}}
      & \frac{1}{m} \mt{\mx{R}} \frac{\partial \vec{F}}{\partial \vec{\Omega}} \\
    0 & 0
      & \frac{\partial \mx{U}}{\partial \vec{\Theta}} \vec{\Omega}
      & \mx{U} \\
    0
      & \minv{\mx{J}} \frac{\partial \vec{\tau}}{\partial \dot{\vec{P}}}
      & \minv{\mx{J}} \frac{\partial \vec{\tau}}{\partial \vec{\Theta}}
      & \minv{\mx{J}} \left[
        \frac{\partial \vec{\tau}}{\partial \vec{\Omega}}
        - \frac{\partial (\vec{\Omega} \crossp \mx{J} \vec{\Omega})}%
        {\partial \vec{\Omega}} \right]
  \end{bmatrix}
\end{equation*}
\fi

\end{document}
% vim:ts=2 sw=2 et spell: