5 |
4 |
5 |
8 | Fig. 1. Quadrotor with the applied forces over the rotors. 9 |
10 | 11 | 12 | ## Description 13 | A simulation of Teppo Luukkonen's paper [Modelling and control of quadcopter](https://scholar.google.com.mx/scholar_url?url=https://www.researchgate.net/file.PostFileLoader.html%3Fid%3D576d16ed93553b24b5721a9a%26assetKey%3DAS%253A376462596165634%25401466767085787&hl=es&sa=X&ei=2i3sYollhJjLBIWnqJgD&scisig=AAGBfm3lte7Y3M6Gfo7VZ3fVAyK0Pd9Wkw&oi=scholarr) in MATLAB/Simulink. 14 | 15 | Since the quadrotor is considered a rigid body, the dynamic equation that rules the quadrotor can be obtained with the Newton-Euler equations 16 | 17 | $$m\ddot{\xi} = G + RT_{B}$$ 18 | 19 | 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. 20 | 21 | In order to control the quadrotor, a simple linear controller was used, the well-known PID controller, that can be described as 22 | 23 | $$u = -K_pe(t) - K_d \dot{e}(t) - K_i \int_0^t{e(\tau)}d\tau$$ 24 | 25 | 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 26 | takes into account the from the instant $0$ to the current instant $t$. 27 | 28 | ### Current Initial Configuration 29 | The quadrotor begins by default with $\phi, \theta, \psi$ and the $altitude$ in a certain position for testing purposes. 30 | 31 | $$ 32 | \begin{align} 33 | \phi &= -0.1\text{ rad} \\ 34 | \theta &= -0.9\text{ rad} \\ 35 | \psi &= -3 \text{ rad} \\ 36 | altitude &= 0\text{m} 37 | \end{align} 38 | $$ 39 | 40 | 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. 41 | 42 | 43 | ### Desired Final Configuration 44 | The goal configuration of the quadrotor can be modified simply by 45 | 46 | $$ 47 | \begin{align} 48 | \phi &= -0.3\text{ rad} \\ 49 | \theta &= 0\text{ rad} \\ 50 | \psi &= -\pi \text{ rad} \\ 51 | altitude &= 5\text{m} 52 | \end{align} 53 | $$ 54 | 55 | 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. 56 | 57 | This is a figure with the trajectory of the drone along the time with the previous initial and desired configurations: 58 | 59 |
60 | 
61 |
64 | Fig. 2. Positions in $x, y$ and $z$ of the quadrotor after 10 seconds of simulations. 65 |
66 | 67 | ### Changing the initial and desired configurations 68 | Both the initial and desired configurations can be modified in the ```parameters.m``` script. 69 | 70 | ## Charts 71 | 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. 72 | 73 |
74 | 
75 | 
76 | 
77 |
80 | 81 | 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$. 82 |
83 | 84 | 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```. 85 | 86 | ## Usage 87 | In order to execute the simulation, the script ```parameters.m``` should be run together with the ```quadrotor_model.slx``` simulink file. 88 | 89 | ## Singularities 90 | 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. 91 | 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. 92 | 93 | ## License 94 | MIT License 95 | 96 | Copyright (c) [2022] [Angelo Espinoza] 97 | 98 | Permission is hereby granted, free of charge, to any person obtaining a copy 99 | of this software and associated documentation files (the "Software"), to deal 100 | in the Software without restriction, including without limitation the rights 101 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 102 | copies of the Software, and to permit persons to whom the Software is 103 | furnished to do so, subject to the following conditions: 104 | 105 | The above copyright notice and this permission notice shall be included in all 106 | copies or substantial portions of the Software. 107 | 108 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 109 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 110 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 111 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 112 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 113 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 114 | SOFTWARE. 115 | -------------------------------------------------------------------------------- /images/quadrotor.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/quadrotor-model-and-control/f8e1fb9e34549246cecbb8449316f327839cff2c/images/quadrotor.jpg -------------------------------------------------------------------------------- /parameters.asv: -------------------------------------------------------------------------------- 1 | clc, clear, close all; 2 | 3 | % Constants 4 | g = 9.81; 5 | m = 0.468; 6 | k = 2.980e-6; % Thrust factor of rotor (depends on density 7 | % geometry, etc) 8 | % to the centre of gravity 9 | l = 0.225; % Linear distance from the centre of the rotor 10 | b = 1.140e-7; % Drag constant 11 | Im = 3.357e-5; % Inertia moment of the rotor 12 | 13 | % Inertia 14 | I_xx = 4.856e-3; % kg*m^2 15 | I_yy = 4.856e-3; % kg*m^2 16 | I_zz = 8.801e-3; % kg*m^2 17 | 18 | % Initial configuration 19 | roll_i = -0.1; % rad 20 | pitch_i = -0.9; % rad 21 | yaw_i = -pi; % rad 22 | altitude_i = 0; % meters 23 | 24 | % Desired configuration 25 | roll_d = -0.3; % rad 26 | pitch_d = 0; % rad 27 | yaw_d = -pi; % rad 28 | altitude_d = 5; % meters 29 | 30 | sim("quadrotor_model") % Initialize Simulink 31 | 32 | % Plot the movements 33 | figure('Name', 'Positions', 'NumberTitle','off') 34 | plot3(x, y, z) 35 | xlabel("Position x") 36 | ylabel("Position y") 37 | zlabel("Position z") 38 | title("Positions") 39 | grid on 40 | 41 | figure('Name', 'Roll angle', 'NumberTitle','off') 42 | plot(tout, roll, tout, ones(size(tout))*roll_d); 43 | xlabel('Time (s)', 'interpreter', 'latex') 44 | l = legend('$\phi$ Current roll angle', '$\phi_d$ Desired roll angle'); 45 | set(l, 'interpreter', 'latex') 46 | 47 | figure('Name', 'Pitch angle', 'NumberTitle','off') 48 | plot(tout, pitch, tout, ones(size(tout))*pitch_d); 49 | xlabel('Time (s)', 'interpreter', 'latex') 50 | l = legend('$\theta$ Current pitch angle', '$\theta_d$ Desired current'); 51 | set(l, 'interpreter', 'latex') 52 | 53 | -------------------------------------------------------------------------------- /parameters.m: -------------------------------------------------------------------------------- 1 | clc, clear, close all; 2 | 3 | % Constants 4 | g = 9.81; 5 | m = 0.468; 6 | k = 2.980e-6; % Thrust factor of rotor (depends on density 7 | % geometry, etc) 8 | % to the centre of gravity 9 | l = 0.225; % Linear distance from the centre of the rotor 10 | b = 1.140e-7; % Drag constant 11 | Im = 3.357e-5; % Inertia moment of the rotor 12 | 13 | % Inertia 14 | I_xx = 4.856e-3; % kg*m^2 15 | I_yy = 4.856e-3; % kg*m^2 16 | I_zz = 8.801e-3; % kg*m^2 17 | 18 | % Initial configuration 19 | roll_i = -0.1; % rad 20 | pitch_i = -0.9; % rad 21 | yaw_i = -3; % rad 22 | altitude_i = 0; % meters 23 | 24 | % Desired configuration 25 | roll_d = -0.3; % rad 26 | pitch_d = 0; % rad 27 | yaw_d = -pi; % rad 28 | altitude_d = 5; % meters 29 | 30 | sim("quadrotor_model") % Initialize Simulink 31 | 32 | % Plot the movements 33 | figure('Name', 'Positions', 'NumberTitle','off') 34 | plot3(x, y, z) 35 | xlabel("Position x") 36 | ylabel("Position y") 37 | zlabel("Position z") 38 | title("Positions") 39 | grid on 40 | 41 | figure('Name', 'Roll angle', 'NumberTitle','off') 42 | plot(tout, roll, tout, ones(size(tout))*roll_d); 43 | xlabel('Time (s)', 'interpreter', 'latex') 44 | l = legend('$\phi$ Current roll angle', '$\phi_d$ Desired roll angle'); 45 | set(l, 'interpreter', 'latex') 46 | 47 | figure('Name', 'Pitch angle', 'NumberTitle','off') 48 | plot(tout, pitch, tout, ones(size(tout))*pitch_d); 49 | xlabel('Time (s)', 'interpreter', 'latex') 50 | l = legend('$\theta$ Current pitch angle', '$\theta_d$ Desired pitch angle'); 51 | set(l, 'interpreter', 'latex') 52 | 53 | figure('Name', 'Yaw angle', 'NumberTitle','off') 54 | plot(tout, yaw, tout, ones(size(tout))*yaw_d); 55 | xlabel('Time (s)', 'interpreter', 'latex') 56 | l = legend('$\psi$ Current yaw angle', '$\psi_d$ Desired yaw angle'); 57 | set(l, 'interpreter', 'latex') -------------------------------------------------------------------------------- /quadrotor_model.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/quadrotor-model-and-control/f8e1fb9e34549246cecbb8449316f327839cff2c/quadrotor_model.slx --------------------------------------------------------------------------------