Quadrotor Model and Control

Fig. 1. Quadrotor with the applied forces over the rotors.

Description

A simulation of Teppo Luukkonen's paper Modelling and control of quadcopter in MATLAB/Simulink.

Since the quadrotor is considered a rigid body, the dynamic equation that rules the quadrotor can be obtained with the Newton-Euler equations

$$m\ddot{\xi} = G + RT_{B}$$

where $m$ is the mass of the quadrotor, $\ddot{\xi}$ is the acceleration of the quadrotor, $G$ is the gravitational constant, $R$ is the rotation matrix, and $T_{B}$ is the thrust of the rotors.

In order to control the quadrotor, a simple linear controller was used, the well-known PID controller, that can be described as

$$u = -K_pe(t) - K_d \dot{e}(t) - K_i \int_0^t{e(\tau)}d\tau$$

where $u$ is the control input, $K_p, K_i$, and $K_d$ are the gains for the proportional, integral and derivative parts, respectively, $e$ is the error of the system, and $\tau$ is the variable of integration that takes into account the from the instant $0$ to the current instant $t$.

Current Initial Configuration

The quadrotor begins by default with $\phi, \theta, \psi$ and the $altitude$ in a certain position for testing purposes.

$$ \begin{align} \phi &= -0.1\text{ rad} \\ \theta &= -0.9\text{ rad} \\ \psi &= -3 \text{ rad} \\ altitude &= 0\text{m} \end{align} $$

where $\phi$ is the roll angle or the rotation around the $x$-axis, $\theta$ is the pitch angle or the rotation around the $y$-axis, and $\psi$ is the yaw angle or the rotation around the $z$-axis.

Desired Final Configuration

The goal configuration of the quadrotor can be modified simply by

$$ \begin{align} \phi &= -0.3\text{ rad} \\ \theta &= 0\text{ rad} \\ \psi &= -\pi \text{ rad} \\ altitude &= 5\text{m} \end{align} $$

where $\phi$ is the roll angle or the rotation around the $x$-axis, $\theta$ is the pitch angle or the rotation around the $y$-axis, and $\psi$ is the yaw angle or the rotation around the $z$-axis.

This is a figure with the trajectory of the drone along the time with the previous initial and desired configurations:

Fig. 2. Positions in $x, y$ and $z$ of the quadrotor after 10 seconds of simulations.

Changing the initial and desired configurations

Both the initial and desired configurations can be modified in the parameters.m script.

Charts

At the end of each simulation charts with the results will be displayed in order to see the behavior of the quadrotor along the time $t$. For this, I've implemented charts of the three Euler angles to see what's going on. It's also worth to point out that the same charts can be seen in the Simulink simulation through the scopes placed at the right part of the diagram.

Fig. 3. Left: Current and desired roll angle $\phi$ and $\phi_d$. Center: Current and desired pitch angle $\theta$ and $\theta_d$. Right: Current and desired yaw angle $\psi$ and $\psi_d$.

It may be the case that $x-y$ and $y-z$ charts get open at the beginning of each simulation, to avoid this, simply delete the X-Y graphs in the diagram at quadrotor_model/Quadrotor System.

Usage

In order to execute the simulation, the script parameters.m should be run together with the quadrotor_model.slx simulink file.

Singularities

Sometimes, due to the quasi-holonomic constraints of the quadrotor, and that we are using Euler angles, the simulation may fall into warnings and errors while choosing the initial or desired configurations. That's why it's recommended to be careful with the election of the configurations. If this may be the case, the best way to start again is to restart with the default configurations previously mentioned. However, this is up to you.

License

MIT License

Copyright (c) [2022] [Angelo Espinoza]

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.