% % MultiRotor.tex % \documentclass{beamer} \usepackage{beamerthemeshadow} \usepackage{listings} \usepackage{times} %\usepackage{algorithm2e} \usepackage{graphics} \usepackage{graphicx} \usepackage{bmpsize} \usepackage{algorithm} \usepackage{algorithmic} \usepackage{pgf,pgfarrows,pgfnodes,pgfautomata,pgfheaps,pgfshade} % \usepackage{amsmath,amssymb} \usepackage[latin1]{inputenc} % \usepackage{colortbl} \usepackage[english]{babel} \newcommand{\erlang}{ \lstset{language=Prolog, basicstyle=\ttfamily\footnotesize,frame=trBL, frameround=fttt}} \newcommand{\erlangsmall}{ \lstset{language=Prolog, basicstyle=\ttfamily\scriptsize,frame=trBL, frameround=fttt}} \newcommand{\clang}{ \lstset{language=Prolog, basicstyle=\ttfamily\footnotesize,frame=trBL, frameround=fttt}} \newcommand{\clangsmall}{ \lstset{language=Prolog, basicstyle=\ttfamily\scriptsize,frame=trBL, frameround=fttt}} \newcommand{\clangtiny}{ \lstset{language=Prolog, basicstyle=\ttfamily\tiny,frame=trBL, frameround=fttt}} \erlang \newcommand\blue[1]{{\color[rgb]{0,0,1}#1}} \newcommand\green[1]{{\color[rgb]{0,0.5,0}#1}} \newcommand\red[1]{{\color[rgb]{1,0,0}#1}} %%\newcommand\black{\color[rgb]{0,0,0}} \newcommand\tape[9]{ \framebox{#1} \framebox{#2} \framebox{#3} \framebox{#4} \framebox{#5} \framebox{#6} \framebox{#7} \framebox{#8} \framebox{#9}} \newcommand\two[2]{ \framebox{#1} \framebox{#2}} \newcommand\six[6]{ \framebox{#1} \framebox{#2} \framebox{#3} \framebox{#4} \framebox{#5} \framebox{#6}} \newcommand\three[3]{ \framebox{#1} \framebox{#2} \framebox{#3}} \title[Control Systems for Multi-rotors]{Control Systems for Multi-rotors\\ Principles, Modeling and Software Design} %% \author[Corrado Santoro] {Corrado Santoro} \date{ } \institute[University of Catania] { \begin{center} \textbf{\red{}ARSLAB - Autonomous and Robotic Systems Laboratory}\\ Dipartimento di Matematica e Informatica - Universit\`{a} di Catania, Italy\\ \texttt{santoro@dmi.unict.it}\\ \end{center} } % Spaces \newcommand{\N}{\vskip 0.3 cm} \newcommand{\n}{\vskip 0.2 cm} \newcommand{\TAB}{\hskip 4 cm} \newcommand{\tab}{\hskip 0.6 cm} \begin{document} %% \frame{\titlepage} %% %% \begin{frame}[fragile] %% \frametitle{Multirotor Structure and Dynamics} %% \begin{block}{} \begin{center} \Large{}Multirotor Structure and Dynamics \end{center} \end{block} %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{Multirotors: definition} %% A \textbf{\red{multirotor}} (a.k.a. ``drone'') is an aerial vehicle characterised by: %% \begin{itemize} \item\footnotesize{}An \textbf{even set} of equal \emph{horizontal propellers} (and motors), $\ge 4$, symmetrically placed in a circular shape \item\footnotesize{}A \textbf{symmetric/balanced} airframe (even if not strictly mandatory) \item\footnotesize{}\textbf{VTOL} (Vertical Take-off and Landing) capabilities \item\footnotesize{}\textbf{Four degrees of freedom}, $XYZ+Heading$ \item\footnotesize{}\textbf{No critical issues} from the mechanical/aerodynamic point of view \item\footnotesize{}Total control in \textbf{software}, no mechanical parts \end{itemize} %% %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{Reference System} %% \begin{itemize} \item\footnotesize{}The \textbf{body reference system} usually employed is the one in figure \item\footnotesize{}The system also define the \textbf{\red{Euler angles}} that represents the \textbf{attitude}: %% \begin{itemize} \item\footnotesize\textbf{roll}, $\phi$ \item\footnotesize\textbf{pitch}, $\theta$ \item\footnotesize\textbf{yaw}, $\psi$ \end{itemize} %% \item\footnotesize{}The \textbf{\red{pose}} of the multirotor is represented by: %% \begin{itemize} %\item\footnotesize$\{\phi,\theta,\psi\}$, in the \textbf{Body frame} \item\footnotesize$\{X,Y,Z,\phi,\theta,\psi\}$, in the \textbf{Earth frame} \end{itemize} %% \end{itemize} %% \begin{center} \includegraphics[scale=0.4]{quad-pic.png} \end{center} %% %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{Airframes and Constraints} %% \begin{itemize} \item\footnotesize{}Motors/propellers must be the \textbf{same} \item\footnotesize{}Motors/propellers must be \textbf{even $\ge 4$} \item\footnotesize{}Motors/propellers must be placed in a \textbf{circular shape} \item\footnotesize{}Propellers must rotate in \textbf{opposite directions} in-pair (third Newton's Law compensation) \item\footnotesize{}Propellers must have \textbf{opposite pitches} in-pair \item\footnotesize{}The number and position of propellers define the \textbf{\red{airframe model}} %% %% \end{itemize} %% \begin{center} \includegraphics[scale=0.4]{airframes.png} \end{center} %% %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{Motion} %% %% \begin{itemize} \item\footnotesize{}Motion is achieved by \textbf{modulating} propeller speeds \item\footnotesize{}We can assume a \emph{virtual pilot} able to give the commands (as in an airplane): \begin{itemize} \item\footnotesize{}\textbf{\red{Thrust}}, the ``power'' to the motors (throttle control) \item\footnotesize{}\textbf{\red{Roll}} and \textbf{\red{Pitch}}, the ``control joke'' \item\footnotesize{}\textbf{\red{Yaw}}, the ``pedals'' \end{itemize} \item\footnotesize{}Let us assume that these commands are \emph{variables} belonging to the ranges: \begin{itemize} \item\footnotesize{}$thrust\_cmd \in [0, TH_{max}]$ \item\footnotesize{}$roll\_cmd \in [-R_{max}, R_{max}]$ \item\footnotesize{}$pitch\_cmd \in [-P_{max}, P_{max}]$ \item\footnotesize{}$yaw\_cmd \in [-Y_{max}, Y_{max}]$ \end{itemize} \item\footnotesize{}These commands must be \emph{``transferred''} to the motors on the basis of the \textbf{specific airframe} \end{itemize} \end{frame} %% \begin{frame}[fragile] %% \frametitle{Motion: Hovering and Z-translation} %% %% \begin{center} \includegraphics[scale=0.3]{quad-motion.pdf} \end{center} %% \footnotesize{}Vertical motion is achieved by keeping all propeller speeds the same and proportional to a \textbf{\red{thrust command}} (we assume 1-proportionality): \begin{eqnarray*} \omega_1 & = & thrust\_cmd\\ \omega_2 & = & thrust\_cmd\\ \omega_3 & = & thrust\_cmd\\ \omega_4 & = & thrust\_cmd\\ \end{eqnarray*} %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{Motion: Yaw rotation in X-shaped quads} %% %% \begin{center} \includegraphics[scale=0.25]{quad-motion-2.pdf} \end{center} %% \footnotesize{}Yaw rotation is achieved by modulating propeller speeds in-pairs $1-3$/$2-4$, proportional to a \textbf{\red{yaw command}}: \begin{eqnarray*} \omega_1 & = & thrust\_cmd - yaw\_cmd\\ \omega_2 & = & thrust\_cmd + yaw\_cmd\\ \omega_3 & = & thrust\_cmd - yaw\_cmd\\ \omega_4 & = & thrust\_cmd + yaw\_cmd\\ \end{eqnarray*} %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{Motion: Roll rotation in X-shaped quads} %% %% \begin{center} \includegraphics[scale=0.25]{quad-motion-3.pdf} \end{center} %% \footnotesize{}Roll rotation is achieved by modulating propeller speeds in-pairs $1-4$/$2-3$, proportional to a \textbf{\red{roll command}}: \begin{eqnarray*} \omega_1 & = & thrust\_cmd - yaw\_cmd + roll\_cmd\\ \omega_2 & = & thrust\_cmd + yaw\_cmd - roll\_cmd\\ \omega_3 & = & thrust\_cmd - yaw\_cmd - roll\_cmd\\ \omega_4 & = & thrust\_cmd + yaw\_cmd + roll\_cmd \end{eqnarray*} %% Roll rotation implies a decomposition of the thrust force: a \textbf{drag} force appears that drives the frame in a \textbf{translated flight} along $Y$ axis %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{Motion: Pitch rotation in X-shaped quads} %% %% \begin{center} \includegraphics[scale=0.25]{quad-motion-4.pdf} \end{center} %% \footnotesize{}Pitch rotation is achieved by modulating propeller speeds in-pairs $1-2$/$3-4$, proportional to a \textbf{\red{pitch command}}: \begin{eqnarray*} \omega_1 & = & thrust\_cmd - yaw\_cmd + roll\_cmd + pitch\_cmd\\ \omega_2 & = & thrust\_cmd + yaw\_cmd - roll\_cmd + pitch\_cmd\\ \omega_3 & = & thrust\_cmd - yaw\_cmd - roll\_cmd - pitch\_cmd\\ \omega_4 & = & thrust\_cmd + yaw\_cmd + roll\_cmd - pitch\_cmd \end{eqnarray*} %% Pitch rotation implies a decomposition of the thrust force: a \textbf{drag} force appears that drives the frame in a \textbf{translated flight} along $X$ axis %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{Motion in Plus-shaped quads} %% %% \begin{center} \includegraphics[scale=0.3]{quad-plus.pdf} \end{center} %% \begin{eqnarray*} \omega_1 & = & thrust\_cmd - yaw\_cmd + pitch\_cmd\\ \omega_2 & = & thrust\_cmd + yaw\_cmd - roll\_cmd\\ \omega_3 & = & thrust\_cmd - yaw\_cmd - pitch\_cmd\\ \omega_4 & = & thrust\_cmd + yaw\_cmd + roll\_cmd\\ \end{eqnarray*} %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{Motion: the Mixer} %% %% \begin{center} \includegraphics[scale=0.4]{mixer.pdf} \end{center} %% \begin{itemize} %% \item\footnotesize{}The \textbf{\red{mixer}} is the software component that \emph{translates} attitude commands to \textbf{motor commands} %% \item\footnotesize{}It depends airframe model and basically implements a matrix $M$ such that %% \end{itemize} %% \[ \footnotesize{}%% \left[ \begin{array}{c}\omega_1\\ \omega_2 \\ \cdots \\ \omega_n\end{array} \right] = M \left[ \begin{array}{c}roll\_cmd\\ pitch\_cmd \\ yaw\_cmd \\ thrust\_cmd\end{array} \right] \] %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{The Mixer: practical aspects} %% %% \begin{center} \includegraphics[scale=0.3]{mixer-2.pdf} \end{center} %% \begin{itemize} %% \item\footnotesize{}Practically, the outputs of the \textbf{mixer} are not the $w_n$ but the \textbf{\red{duty cycle}} values of the motor PWM drivers %% \end{itemize} %% \[ \footnotesize{}%% \left[ \begin{array}{c}PWM_1\\ PWM_2 \\ \cdots \\ PWM_n\end{array} \right] = M \left[ \begin{array}{c}roll\_cmd\\ pitch\_cmd \\ yaw\_cmd \\ thrust\_cmd\end{array} \right] \] %% \begin{itemize} %% \item\footnotesize{}PWM values are then \textbf{saturated} using a technique that avoids certain side-effects %% \end{itemize} \end{frame} %% \begin{frame}[fragile] %% \frametitle{The Mixer: Saturation} %% %% \begin{itemize} %% \item\footnotesize{}Each PWM output cannot be more than a maximum ($PWM_{MAX}$, i.e.~the 100\% duty cycle) %% \item\footnotesize{}Each PWM output cannot be less than the minimum value that stops the propeller: this is required to avoid multirotor \textbf{flip} %% \item\footnotesize{}The constraints above must not affect the displacement imposed, otherwise the control could not work %% \end{itemize} \begin{algorithmic}[1] \scriptsize{}% \red{ %\caption{PROFETA Execution Loop}\label{fig:profeta_machine} \STATE{$M \leftarrow \max\{PWM_1, PWM_2, \ldots, PWM_n\}$} \STATE{$m \leftarrow \min\{PWM_1, PWM_2, \ldots, PWM_n\}$} \IF{$M > PWM_{MAX}$} \STATE{$D \leftarrow M - PWM_{MAX}$} \STATE{$PWM_i \leftarrow PWM_i - D$} \STATE{$M \leftarrow PWM_{MAX}$} \STATE{$m \leftarrow m - D$} \ENDIF \IF{$M - m > \Delta PWM_{MAX}$} \STATE{$m \leftarrow M - \Delta PWM_{MAX}$} \ENDIF \IF{$PWM_i < m$} \STATE{$PWM_i \leftarrow m$} \ENDIF} \end{algorithmic} %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{The Control System} %% \begin{block}{} \begin{center} \Large{}The Control System of a Multirotor \end{center} \end{block} %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{Rate and Attitude Control} %% %% \begin{itemize} %% \item\footnotesize{}The mixer outputs PWM values and does not have control on the real forces of the propellers\N %% \item\footnotesize{}In order to ensure \textbf{stability}, proper sensors must be employed that detects the \emph{attitude} of the multirotor\N %% \item\footnotesize{}The control of stability is achieved by means of two control loops: \begin{itemize} \item\footnotesize{}\textbf{\red{Rate Control}}, controls \emph{angular speeds} $\dot{\phi}, \dot{\theta}, \dot{\psi}$, by means of a 3-axis gyro \item\footnotesize{}\textbf{\red{Attitude Control}}, controls \emph{Euler angles} $\phi, \theta, \psi$, by means of a 6-DOF or 9-DOF IMU \end{itemize} %% \end{itemize} \end{frame} %% \begin{frame}[fragile] %% \frametitle{Rate Control: the ``Acro'' Mode} %% %% \begin{center} \includegraphics[scale=0.35]{rate-control.pdf} \end{center} %% %% \begin{itemize} %% \item\footnotesize{}The \textbf{\red{Rate Control}} module performs a PID control on angular rates $\dot{\phi}, \dot{\theta}, \dot{\psi}$ on the basis of: %% \begin{itemize} \item\footnotesize{}\textbf{\red{Target Rates}}, given as input \item\footnotesize{}\textbf{\red{Current Rates}}, given by the gyro \end{itemize} %% \item\footnotesize{}When the target rates are given by the RC command, the mode is called \textbf{acrobatic} \end{itemize} \end{frame} %% \begin{frame}[fragile] %% \frametitle{Rate Control: Practical Aspects} %% %% \begin{center} \includegraphics[scale=0.3]{rate-control.pdf} \end{center} %% %% \begin{itemize} %% \item\footnotesize{}\textbf{Rate Controllers} are usually \textbf{\red{PI}} or \textbf{\red{PID}} controllers (the derivative part is often filtered by a LPF) %% \item\footnotesize{}Outputs are \textbf{\red{saturated}} to a specific value (usually 100\% of PWM duty cycle) %% \item\footnotesize{}The \textbf{\red{anti-wind-up}} optimisation is included %% \item\footnotesize{}Since controllers must operate only ``in flight'', they are ``activated'' only when the \textbf{thrust} command is greater than a \textbf{\red{threshold}} %% \end{itemize} \end{frame} %% \begin{frame}[fragile] %% \frametitle{Rate Control: Implementation} %% %% \begin{center} \includegraphics[scale=0.3]{rate-control.pdf} \end{center} %% %% \begin{itemize} %% \item\footnotesize{}\textbf{Rate Control} module is implemented as a \textbf{\red{periodic task}} triggered by: %% %% \begin{itemize} \item\footnotesize{}A \textbf{\red{Timer}}, with a specific period \item\footnotesize{}The \textbf{\red{gyro sampling frequency}} \end{itemize} %% \item\footnotesize{}Periods are in the order of $200-500 Hz$ %% \item\footnotesize{}The code implements the classical PID algorithm with anti-wind-up %% %% \end{itemize} \end{frame} %% \begin{frame}[fragile] %% \frametitle{Rate and Attitude Control} %% %% \begin{itemize} %% \item\footnotesize{}Rate control ensures that real angular rates are the one desired but do not provide guarantees on the \textbf{\red{attitude}}, i.e.~that the pose of the airframe is a specific one $\{\overline{\phi},\overline{\theta},\overline{\psi}\}$\N %% \item\footnotesize{}Moreover, starting from an horizontal pose \red{$\phi=0, \theta = 0$}, if a ``glitch'' moves suddenly the pitch angle to \red{$\theta = 10^{\circ}$}, the system is \textbf{\red{stable}} from the \red{rate control point of view} (there are no rotations), but (probably) the \red{attitude is not the desired one}\N %% \item\footnotesize{}An \textbf{\red{Attitude Controller}} is required, which drives the Rate Controllers and ensures the respect of a certain pose, from the \red{Euler angles} point of view\N %% \item\footnotesize{}To this aim, the \textbf{\red{current attitude}} is \textbf{estimated} by integrating data from \emph{gyros}, \emph{accelerometers} and \emph{magnetometers}. \end{itemize} \end{frame} %% \begin{frame}[fragile] %% \frametitle{Attitude Control: Basic Schema} %% %% \begin{center} \includegraphics[scale=0.35]{attitude-control.pdf} \end{center} %% %% %% \begin{itemize} %% \item\footnotesize{}\textbf{Attitude Controllers} are (usually) simple \textbf{\red{P}} controllers %% \item\footnotesize{}Outputs are \textbf{\red{saturated}} to a \red{maximum angular speed}, determined experimentally %% \item\footnotesize{}\textbf{Attitude Control} module is implemented as a \textbf{\red{periodic task}} (like the Rate Control), with a period same as to (or greater than) that of Rate Control %% %% \end{itemize} \end{frame} %% \begin{frame}[fragile] %% \frametitle{Attitude Estimation} %% \begin{block}{} \begin{center} \Large{}Attitude Estimation \end{center} \end{block} %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{Attitude Estimator} %% %% \begin{itemize} %% \item\footnotesize{}The most critical part of Attitude Control is the \textbf{Sensor Fusion} algorithm that implements the \textbf{\red{Attitude Estimator}}\N %% \item\footnotesize{}The literature reports a plethora of solutions: \begin{itemize} \item\footnotesize{}\red{Kalman Filters} \item\footnotesize{}\red{Complementary Filters} \item\footnotesize{}\red{Direction Cosine Matrix Algorithm} \item\footnotesize{}\red{Gradient Descend} \item\footnotesize{}\red{...} \end{itemize} %% \item\footnotesize{}(Some) quality factors of the estimator: %% \begin{itemize} \item\footnotesize{}\textbf{\red{Resilience to vibrations}} \item\footnotesize{}\textbf{\red{Difference w.r.t. the real attitude}} \item\footnotesize{}\textbf{\red{Rate of convergence}} \end{itemize} %% \end{itemize} %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{Attitude Estimator: Basics} %% %% \begin{itemize} %% \item The basic working scheme of the estimator is the following:\N %% \begin{enumerate} \item\footnotesize{}\red{Wait sampling period} \item\footnotesize{}\red{Update Euler angles using data from gyroscopes} by performing discrete integration \item\footnotesize{}\red{Adjust Pitch and Roll on the basis of data from accelerometers} ($\overrightarrow{g}$ vector) \item\footnotesize{}\red{Adjust Yaw on the basis of data from magnetometers} ($\overrightarrow{N}$ vector) \end{enumerate}\N %% \item The various algorithms differ in the way in which adjustments (steps 2 and 3) are performed %% \end{itemize} %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{The Complementary Filter Algorithm} %% \begin{center} \includegraphics[scale=0.22]{CompFilter.pdf} \end{center} %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{The Direction Cosine Matrix Algorithm} %% %% \begin{itemize} \item\footnotesize{}The DCM is the \textbf{\red{rotation matrix}} from ``Earth reference'' and ``Body reference'' of a rigid body whose attitude is expressed by means of Euler angles $\theta, \phi, \psi$ \end{itemize} %% \begin{block}{Direction Cosine Matrix} \[ DCM = \left ( \begin{array}{ccc} %% c \theta c \psi & s \phi s \theta c \psi - c \phi s \psi & c \phi s \theta c \psi + s \phi s \psi \\ %% c \theta s \psi & s \phi s \theta s \psi + c \phi c \psi & c \phi s \theta s \psi - s \phi c \psi \\ %% - s \theta & s \phi c \theta & c \phi c \theta \\ \end{array} \right ) \] \[ s = \sin, ~~ c = \cos \] %% \end{block} %% \begin{itemize} \item\footnotesize{}A vector $v' = (x',y',z')$ in \textbf{local (body)} frame is translated into \textbf{global (Earth)} frame by multiplying it by the DCM: $v = DCM \cdot v'$ \end{itemize} %% \end{frame} %% %% %% \begin{frame}[fragile] %% \frametitle{Direction Cosine Matrix} %% %% \begin{itemize} \item\footnotesize{}The DCM has some properties \item It is \textbf{\red{orthogonal}}, its transpose is equal to its inverse: \begin{eqnarray*} DCM^{-1} & = & DCM^{T}\\ DCM \cdot DCM^{T} & = & I \end{eqnarray*} \item Orthogonality implies that the columns (rows) \begin{itemize} \item\footnotesize{}are orthogonal vectors in-pair $\rightarrow$ their \textbf{cross product is zero} \item are vector with module equal to $1$ \end{itemize} \item\textbf{\red{Such properties must be always respected}} \end{itemize} \end{frame} %% %% %% \begin{frame}[fragile] %% \frametitle{The DCM Algorithm} %% \begin{center} \includegraphics[scale=0.22]{DCM.pdf} \end{center} %% \end{frame} %% %% %% \begin{frame}[fragile] %% \frametitle{Attitude Estimators: Improving the Quality} %% %% \begin{itemize} %% \item\footnotesize{}\textbf{\red{Sensor Calibration}} \begin{itemize} \item\footnotesize{}\textbf{Gyros:} get (and store) \textbf{offsets} $b_w$ and remove them from measures: \red{$\overline{\omega} = \omega - b_w$} \item\footnotesize{}\textbf{Accelerometers:} get (and store) \textbf{offsets} $b_a$ and rotation matrix $R_A$ remove them from measures: \red{$\overline{a} = R_A \cdot (a - b_a)$} \item\footnotesize{}\textbf{Magnetometers:} get (and store) \textbf{offsets} $b_m$ and rotation matrix $R_M$ remove them from measures: \red{$\overline{m} = R_M \cdot (m - b_m)$} \end{itemize}\N %% \item\footnotesize{}\textbf{\red{Sensor Filtering}}, low-pass filters (with order $\ge 2$) are often used to reduce the effect of motor/propeller vibrations\N %% \item\footnotesize{}\textbf{\red{Mechanical Dampers}}, to decouple the flight control board from the airframe to reduce the effect of motor/propeller vibrations\N %% \item\footnotesize{}\textbf{\red{Dynamic $g$ compensation}}, the contribution of accelerometers is weighted according to the error $|g - \|a\| |$ in order to reduce the effect, on $a$, of body translations\N %% \end{itemize} \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{Summary of Basic Software Modules} %% \begin{block}{} \begin{center} \Large{Summary of basic Software Modules} \end{center} \end{block} %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{In Summary... the Basic Software Modules} %% \begin{center} \includegraphics[scale=0.4]{modules_1.pdf} \end{center} %% %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{Other Kind of Controls} %% \begin{block}{} \begin{center} \Large{Other Kind of Controls} \end{center} \end{block} %% \end{frame} %% %% %% \begin{frame}[fragile] %% \frametitle{Altitude Control} %% \begin{center} \includegraphics[scale=0.4]{altitude_control.pdf} \end{center} %% \begin{block}{\textbf{Altitude Control}} \begin{itemize} \item\footnotesize{}\textbf{\blue{Altitude $Z$}} is estimated by means of proper sensors (barometer, in some case integrated with measures from accelerometers, ultrasonic sensors, etc.) \item\footnotesize{}The \textbf{\blue{Vertical Speed $V_z$}} is determined by a derivative of the altitude \item\footnotesize{}Control is performed by means of two control loops that drive the \emph{thrust command} \begin{itemize} \item\footnotesize{}An inner \textbf{\blue{PI(D)}} speed controller driving the thurst \item\footnotesize{}An outer \textbf{\blue{P-(FF)}} position controller driving the speed controller \end{itemize} \end{itemize} \end{block} \end{frame} %% %% %% \begin{frame}[fragile] %% \frametitle{Position Control} %% %% \begin{block}{\textbf{Pose Estimator}} \begin{itemize} \item\footnotesize{}GPS signal is used to determine (and control) the \textbf{pose} in the Earth frame \item An \textbf{\red{EKF estimator}} is used to fuse data from GPS and IMU to estimate: \begin{itemize} \item\footnotesize{}\red{Position $\{X, Y, Z\}$}, in NED coordinates \item\footnotesize{}\red{Speeds $\{V_x, V_y, V_z\}$} \item\footnotesize{}\red{Euler Angles $\{\phi, \theta, \psi\}$} \end{itemize} \item The EKF is complex and CPU-time consuming (in PX4, it is a 22-state estimator) \end{itemize} \end{block} \end{frame} %% %% %% \begin{frame}[fragile] %% \frametitle{Position Control} %% \begin{center} \includegraphics[scale=0.4]{position_control.pdf} \end{center} %% \begin{block}{\textbf{Position Control}} \begin{itemize} \item\footnotesize{}Position Control is performed by means of two pair of control loops (\red{North and East}) that drive the \emph{\red{target roll and pitch}} of the \textbf{attitude controller} \begin{itemize} \item\footnotesize{}An inner \textbf{\blue{PI(D)}} speed controller driving the target attitude (roll and pitch) \item\footnotesize{}An outer \textbf{\blue{P-(FF)}} position controller driving the speed controller \end{itemize} \end{itemize} \end{block} \end{frame} %% %% %% \begin{frame}[fragile] %% \frametitle{The Ground Control Station} %% \begin{block}{} \begin{center} \Large{Ground Control Station} \end{center} \end{block} %% %% \end{frame} %% %% %% \begin{frame}[fragile] %% \frametitle{The Ground Control Station} %% \begin{center} \includegraphics[scale=0.15]{TabletFly.png}~~%% \includegraphics[scale=0.15]{apm_summary.png}~~%% \includegraphics[scale=0.05]{gcs_data.png} \end{center} %% \begin{block}{\textbf{GCS}} \begin{itemize} \item\footnotesize{}Most of the UAV flight stacks have the possibility of connecting a setup, telemetry and maintenance GUI called \textbf{\red{Ground Control Station}} \item\footnotesize{}It can be used for \begin{itemize} \item\footnotesize{}Configuring the UAV and calibrating the sensor \item\footnotesize{}Monitoring telemetry data \item\footnotesize{}Planning the missions \item\footnotesize{}Modifying all the parameters (gains of controllers or of the sensor fusion algorithms, sensor configuration, RC commands, etc.) \end{itemize} \end{itemize} \end{block} \end{frame} %% %% %% \begin{frame}[fragile] %% \frametitle{The Ground Control Station} %% \begin{center} \includegraphics[scale=0.15]{TabletFly.png}~~%% \includegraphics[scale=0.15]{apm_summary.png}~~%% \includegraphics[scale=0.05]{gcs_data.png} \end{center} %% \begin{block}{\textbf{MAVLink}} \begin{itemize} \item\footnotesize{}Communication between GCS and the Flight Stack is performed through a standard protocol called \textbf{\red{MAVLink}} \item\footnotesize{}It is designed to be used in \textbf{serial links} (wired or radio) or \textbf{TCP/UDP} channels \item\footnotesize{}It can be used not only for GCS-like activities but also to control the UAV through an \textbf{external on-board computer} in order to do flight tasks: \begin{itemize} \item\footnotesize{}\red{Arming/Disarming} \item\footnotesize{}\red{Triggering take-off and land} \item\footnotesize{}\red{Sending specific set-points (NED positions, or $V_x, V_y, V_z$ speeds)} \item\footnotesize{}\red{Sending and triggering a mission} \end{itemize} \end{itemize} \end{block} \end{frame} %% %% %% \begin{frame}[fragile] %% \frametitle{Overall Software Modules} %% \begin{center} \includegraphics[scale=0.4]{modules_2.pdf} \end{center} %% %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{The PX4 Autopilot} %% \begin{block}{} \begin{center} \Large{}The PX4 Autopilot \end{center} \end{block} %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{The PX4 Autopilot} %% %% \begin{block}{} \begin{itemize} %% \item\footnotesize{}The \textbf{\red{PX4 Autopilot}} is a flight control software designed to drive a large set of \emph{\red{autonomous vehicles}}, including ground and aerial platforms\N %% \item It is entirely written in C++ and includes two basic parts: \begin{itemize} %% \item\footnotesize{}\textbf{\red{PX4 Flight Stack}}: modules that implement control algorithms, estimators, etc. for manual and autonomous flight %% \item\footnotesize{}\textbf{\red{PX4 Middleware}}: infrastructure for communication among all software modules of the Flight Stack \end{itemize}\N %% \item\footnotesize{}PX4 runs on top of \textbf{\red{NuttX}}, a Unix-like real-time operating system (developed by Gregory Nutt) that provides a support for: \begin{itemize} %% \item\footnotesize{}\red{pre-emptive thread scheduling} \item\footnotesize{}\red{device drivers} \item\footnotesize{}\red{virtual file system} \item\footnotesize{}\red{a minimal shell} %% \end{itemize} %% \end{itemize} \end{block} %% \end{frame} %% \begin{frame}[fragile] %% \frametitle{PX4 Middleware} %% %% \begin{center} \includegraphics[scale=0.35]{modules_2.pdf} \end{center} %% %% \begin{block}{} \begin{itemize} %% \item\footnotesize{}Since software modules need to interact to each other, a \textbf{communication middleware} is provided called \textbf{\red{uORB}}\N %% \item\footnotesize{}It is based on a \textbf{publisher/subscriber} mechanism\N %% \item\footnotesize{}Data is identified by a \textbf{topic}, so publishing, subscribing and data copy is handled \emph{``by topic''}\N %% \item\footnotesize{}Structures of messages used in PX4 are defined in some text files in the directory \textbf{\texttt{\red{msg}}} %% \end{itemize} \end{block} %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{PX4 uORB Messages} %% %% \footnotesize{}The \textbf{\texttt{\red{msg}}} directory %% \clangtiny{}%% \begin{lstlisting} ... output_pwm.msg parameter_update.msg position_setpoint.msg pwm_input.msg sensor_accel.msg sensor_baro.msg sensor_gyro.msg sensor_mag.msg ... \end{lstlisting} %% \footnotesize{}The \textbf{\texttt{\red{sensor\_mag.msg}}} file %% \clangtiny{}%% \begin{lstlisting} uint64 timestamp uint64 error_count float32 x float32 y float32 z float32 range_ga float32 scaling float32 temperature int16 x_raw int16 y_raw int16 z_raw uint32 device_id \end{lstlisting} %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{PX4 Flight Stack} %% %% \begin{block}{\textbf{Source Directory Structure}} %% \begin{itemize} %% \item\footnotesize{}\textbf{\texttt{\red{drivers}}}, abstraction layer for physical equipment (sensors, motors, etc.), and specific device drivers for each supported equipment\N %% \item\footnotesize{}\textbf{\texttt{\red{modules}}}, all software modules performing the control of the vehicle\N %% \item\footnotesize{}\textbf{\texttt{\red{lib}}}, additional libraries for scalar and matrix math, coordinate system handling, control system modules, filters, etc.\N %% \end{itemize} %% \end{block} %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{PX4 Driver Layer} %% %% \begin{block}{\textbf{Device Drivers}} %% \begin{itemize} %% \item\footnotesize{}PX4 \textbf{\red{Device drivers}} must export a POSIX-compliant interface, with callbacks for functions as \emph{open}, \emph{close}, \emph{read}, \emph{ioctl}\N %% \item\footnotesize{}Devices handled are mainly \textbf{sensors} and the current version of PX4 supports a large number of them: %% \end{itemize} \clangtiny{}%% \begin{lstlisting} ./src/drivers: gps mpu6050__ hc_sr04 mpu6500 hmc5883 mpu9250 irlock ms5611 l3gd20 oreoled led pca8574 lis3mdl pca9685 ll40ls pwm_input lps22hb pwm_out_sim lsm303d sf0x lsm6ds33 sf10a mb12xx srf02 md25 srf02_i2c meas_airspeed uart_esc mpu6000 mpu6050 \end{lstlisting} %% \end{block} %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{PX4 Device Driver and Sensors} %% %% \begin{block}{\textbf{Multiple Sensors Handling}} %% \begin{itemize} %% \item\footnotesize{}The PX4 firmware is able to use \textbf{\red{multiple sensors}}, also of the same type (e.g.~two or more gyros, accelerometers, magnetometers, etc.)\N %% \item\footnotesize{}Data relevant to the same sensor type are gathered altogether\N %% \item\footnotesize{}A \textbf{\red{data quality evaluator}} algorithm is employed in order to detect the ``best'' data read and use it in subsequent computations\N %% \item\footnotesize{}The evaluator is based on comparing the data stream with the same stream filtered by a LPF and computing the error variance %% \end{itemize} %% \end{block} %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{PX4 Modules} %% %% \begin{block}{\textbf{The \texttt{modules} directory}} %% \begin{itemize} %% \item\footnotesize{}PX4 \textbf{\red{modules}} are the main control blocks of the autopilot\N %% \item\footnotesize{}Each module is a NuttX task that is started at system startup and implements an infinite loop performing the activities:\N \begin{enumerate} \item\footnotesize{}\red{waiting for the sampling period} \item\red{retrieving subscribed data from uORB} \item\red{executing the specific computation} \item\red{publishing the result to uORB} \end{enumerate} %% \end{itemize} %% \end{block} %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{PX4 Modules} %% %% \begin{block}{\textbf{The \texttt{modules} directory}} %% \begin{itemize} %% \item\footnotesize{}Modules include: \begin{itemize} \item\scriptsize{}\textbf{\texttt{\red{attitude\_estimator\_ekf}}}, EKF for attitude estimation \item\scriptsize{}\textbf{\texttt{\red{attitude\_estimator\_q}}}, complementary filter for attitude estimation \item\scriptsize{}\textbf{\texttt{\red{commander}}}, sensor calibration routines and GCS command handling (through MAVLink) \item\scriptsize{}\textbf{\texttt{\red{ekf\_att\_pos\_estimator}}}, EKF for position and attitude estimation \item\scriptsize{}\textbf{\texttt{\red{fw\_att\_control}}}, attitude (and rate) controllers for fixed-wing UAVs \item\scriptsize{}\textbf{\texttt{\red{fw\_pos\_control\_l1}}}, position controllers for fixed-wing UAVs \item\scriptsize{}\textbf{\texttt{\red{mavlink}}}, MAVLink protocol routines \item\scriptsize{}\textbf{\texttt{\red{mc\_att\_control}}}, attitude (and rate) controllers for multirotor UAVs \item\scriptsize{}\textbf{\texttt{\red{mc\_pos\_control}}}, position controllers for multirotor UAVs \item\scriptsize{}\textbf{\texttt{\red{navigator}}}, controller for handling autonomous missions \item\scriptsize{}\textbf{\texttt{\red{sdlog2}}}, the logger \item\scriptsize{}\textbf{\texttt{\red{segway}}}, attitude controllers for a segway \item\scriptsize{}\textbf{\texttt{\red{systemlib}}}, system modules, including the \textbf{mixer} \end{itemize} %% \end{itemize} %% \end{block} %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{The CDrone Flight Stack} %% \begin{block}{} \begin{center} \Large{}The CDrone Flight Stack \end{center} \end{block} %% \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{CDrone Flight Stack} %% %% \begin{block}{\textbf{CDrone Basics}} %% \begin{itemize} %% \item\footnotesize{}It is a flight stack developed at the \textbf{DMI@UNICT} for educational purposes \item Initially designed in \textbf{C} for \textbf{Microchip dsPIC33F} family MCUs \item Then rewritten in \textbf{\red{C++}} and ported to the \textbf{\red{STM32 architecture}} (STM32F401RE) \item It runs in \textbf{bare metal}, with a very light HAL layer written in C++ \item It includes only the \textbf{basic modules} of a UAV control system (those in figure + the MAVLink interface) %% \end{itemize} %% \end{block} %% \begin{center} \includegraphics[scale=0.35]{modules_1.pdf} \end{center} \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{CDrone HAL} %% %% \begin{block}{\textbf{Peripheral and Task Layers}} %% \begin{itemize} %% \item\footnotesize{}CDrone is strongly object-based, so everything is defined as a \textbf{class} \item The \textbf{\red{Peripheral layer}} includes some (abstract and concrete) classes each representing a specific peripheral \item The \textbf{\red{Task layer}} includes the abstract class \textbf{\texttt{PeriodicTask}} (the base for the implementation of any periodic task) and the (non preemptive) scheduler \item Scheduling is triggered by a timer running at 400 Hz %% \end{itemize} %% \end{block} %% \begin{center} \includegraphics[scale=0.35]{CDrone_HAL.pdf} \end{center} \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{CDrone HAL} %% %% \begin{block}{\textbf{Sensor Drivers}} %% \begin{itemize} %% \item\footnotesize{}IMU Sensors are represented by a generic \textbf{\red{IMU}} abstract class that must be extended into the class that implements the code to handle specific sensors \item A \textbf{\red{IMU\_X\_NUCLEO}} class that drives (via I$^2$C) a X-NUCLEO-IKS01A1 add-on board %% \end{itemize} %% \end{block} %% \begin{center} \includegraphics[scale=0.35]{cdrone_drivers.pdf} \end{center} \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{CDrone HAL} %% %% \begin{block}{\textbf{Control System Layer}} %% \begin{itemize} %% \item\footnotesize{}Some (abstract and concrete) classes implementing %% \begin{itemize} \item\footnotesize{}the \textbf{\red{PID}} algorithm (with derivative low-pass filter, anti-wind-up and feedforward) \item some filters, a $2^{nd}$ order \textbf{\red{LowPass}} filter, and a $4^{nd}$ order \textbf{\red{Chebysev}} low-pass filter %% \end{itemize} \end{itemize} %% \end{block} %% \begin{center} \includegraphics[scale=0.35]{control_layer.pdf} \end{center} \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{CDrone Flight Control Classes} %% %% \begin{block}{} %% \begin{itemize} %% \item\scriptsize{}\textbf{\texttt{\red{RemoteControl}}}, interface with the RC \item\textbf{\texttt{\red{RateControl}}}, angular rate control algorithms \item\textbf{\texttt{\red{AttitudeControl}}}, control algorithms on Euler angles \item\textbf{\texttt{\red{AirFrame}}}, abstract class representing an airframe \item\textbf{\texttt{\red{AirFrameQuadX}}}, class representing an X-shaped quadcopter \item\textbf{\texttt{\red{AHRS}}}, abstract class representing a generic sensor fusion algorithm \item\textbf{\texttt{\red{ComplementaryFilter}}}, the complementary filter sensor fusion algorithm \item\textbf{\texttt{\red{DCM}}}, the DCM sensor fusion algorithm \item\textbf{\texttt{\red{MAVLinkReceiver}}}, MAVLink command handler \end{itemize} %% \end{block} %% \begin{center} \includegraphics[scale=0.3]{cdrone_control.pdf} \end{center} \end{frame} %% %% \begin{frame}[fragile] %% \frametitle{CDrone: Statistics} %% %% \begin{block}{} %% \begin{itemize} %% \item\footnotesize{}\textbf{\red{Loop-time}}: $2.5ms$\N \item\textbf{\red{I$^2$C Sensor Polling}}: $930 \mu s$\N \item\textbf{\red{DCM Sensor Fusion, Rate and Attitude Control}}: $410 \mu s$ without FPU\N \item\textbf{\red{DCM Sensor Fusion, Rate and Attitude Control}}: $290 \mu s$ with FPU\N \item\textbf{\red{Total processing time}}: $1.34 ms$ without FPU\N \item\textbf{\red{Total processing time}}: $1.22 ms$ with FPU\N %% \end{itemize} %% \end{block} %% \end{frame} %% %% \frame{\titlepage} %% %% %% \end{document} %% %%