SciELO - Scientific Electronic Library Online

 
vol.22 número2Incrementando la flexibilidad del diseño del compensador de filtro comb mediante optimización de enjambre de partículasAlgoritmo interactivo para decisiones con varios objetivos, riesgo e incertidumbre índice de autoresíndice de materiabúsqueda de artículos
Home Pagelista alfabética de revistas  

Servicios Personalizados

Revista

Articulo

Indicadores

Links relacionados

  • No hay artículos similaresSimilares en SciELO

Compartir


Ingeniería, investigación y tecnología

versión On-line ISSN 2594-0732versión impresa ISSN 1405-7743

Ing. invest. y tecnol. vol.22 no.2 Ciudad de México abr./jun. 2021  Epub 30-Jul-2021

https://doi.org/10.22201/fi.25940732e.2021.22.2.015 

Artículos

Control and signal filtering system for a quadcopter; analysis, comparison and implementation via low-cost IMU and microcontroller

Sistema de control y filtrado de señales para un quadrotor; análisis, comparación e implementación mediante IMU de bajo costo y microcontrolador

Jacobo Marcos Allende-Peña1 
http://orcid.org/0000-0003-1227-0449

Salvador Antonio Rodríguez-Paredes2 
http://orcid.org/0000-0002-1725-9874

Bernardino Benito Salmerón-Quiroz3 
http://orcid.org/0000-0001-9028-8287

1 Tecnológico de Estudios Superiores de Tianguistenco Estado de México. Instituto Politécnico Nacional, Sección de Estudios de Posgrado e Investigación de la ESIME Azcapotzalco, México. E-mail: jacobmarcoap@gmail.com

2 Instituto Politécnico Nacional, México. Sección de Estudios de Posgrado e Investigación de la ESIME Azcapotzalco. E-mail: sarodriguezp@ipn.mx

3 Instituto Politécnico Nacional, México. Sección de Estudios de Posgrado e Investigación de la ESIME Azcapotzalco. E-mail: bsalmeron@ipn.mx


Abstract

This work presents the development, analysis and comparison of dynamic models, to implement in a simple way a filtering and control system for a quadcopter through the use of a microcontroller. A non-linear dynamic model obtained from the Euler Lagrange equations is approached; also a simplified non-linear model from the first model and a linear model are obtained. Subsequently, making use of each model, controllers are designed using the computed torque controller technique, then, their performance index is obtained through numerical simulations applied to the complete non-linear model to compare their response. Afterward, with the various models and through observers and Kalman filters, signal filtering systems are synthesized for a low-cost Inertial Measurement Unit (IMU), the filtering results are also compared using a performance index. Additionally, the proposed controller is compared in simulation with a fuzzy logic controller. Finally, the selection of the model, the controller and filtering are validated through experimentation with a quadcopter prototype developed by the authors, based on an experimental platform with four rotors, a fiberglass structure, a microcontroller and an IMU MPU 6050.

Keywords: Quadcopter; model analysis; computed torque controller; Kalman filter; observer; low-cost IMU; implementation; microcontroller

Resumen

Se presenta el desarrollo, análisis y comparación de modelos dinámicos, para implementar de forma simple un sistema de filtrado y de control para una aeronave tipo quadrotor mediante el uso de microcontrolador. Se aborda un modelo dinámico no lineal obtenido de las ecuaciones de Euler Lagrange, así como un modelo no lineal simplificado a partir del primer modelo y, a su vez, un modelo lineal. Posteriormente, haciendo uso de cada modelo, se diseñan controladores mediante la técnica de par calculado, luego se obtienen sus índices de desempeño mediante simulaciones numéricas aplicadas al modelo no lineal completo, con el fin de comparar su respuesta. Además, con los diversos modelos y mediante observadores y filtros de Kalman, se sintetizan propuestas de filtrado de señales para una Unidad de Medición Inercial (IMU) de bajo costo, también se comparan los resultados del filtrado mediante un índice de desempeño. Adicionalmente se compara en simulación el controlador propuesto con un controlador por lógica difusa. Por último se valida la selección del modelo, el controlador y filtrado mediante experimentación con un prototipo de quadrotor desarrollado por los autores, basado en una plataforma experimental de cuatro rotores, una estructura de fibra de vidrio, un microcontrolador y una IMU MPU 6050.

Descriptores: Quadrotor; análisis de modelos; control par calculado; filtro de Kalman; observador de estado; IMU de bajo costo; implementación; microcontrolador

Introduction

In recent years, the rise of unmanned aerial vehicles (UAV) has had a great impact on society so the area of possible applications increases continuously, for example: in the field of the military, trade, the search, rescue, dangerous environment monitoring, product delivery, as well as others (Floreano & Wood, 2015; Gupta et al., 2013; Cárdenas, 2015; Gutiérrez et al., 2017.

One of the main problems, when is generating a control system for an aerial vehicle, is to develop a dynamic model that describes the real behavior of the aircraft in the most precise way possible and at the same time; keep the model simple to design, analyze and investigate various control strategies, signal filtering, as well as implement the controller and filtering system in a digital system in a simple way. The quadcopter mathematical model is a set of equations that allows representing in detail the dynamic behavior of the quadcopter in flight; one of the main ways to approximate the dynamic model is applying the Euler-Lagrange methodology, which consists in observing the angular velocities of the vehicle, from a reference frame, movable but aligned to an inertial frame (Sadr et al., 2014; Rodríguez et al., 2016; Balasubramanian & Vasantharaj, 2013; Nonami et al., 2010; Beard & McLain, 2012; Garcia et al., 2006; Castillo et al., 2007; Carrillo et al., 2012; Raffo, 2007), this methodology incorporates the inertial and gyroscopic effects of the aircraft structure into the model. On the other hand, there is a model developed using Newton-Euler, which observes the angular velocities of the quadrotor from a mobile frame defined in the center of rotation of the aircraft (Bouabdallah & Siegwart, 2007; Falconi & Melchiorri, 2012; Elruby et al., 2012; de Jesus Rubio et al., 2014; Paiva, 2016; Bouabdallah & Siegwart, 2004; Hossain et al., 2010; Patete & Erazo, 2016; Agho, 2017), in this model, it is common to observe that the gyroscopic and inertial effects of the aircraft structure are despised, however, the gyroscopic effect of the rotation of the propellers is incorporated. Also, there are linear approximations, in which the flight dynamics around an operating point are frequently approximated (Sabatino, 2015; Roldan, 2016; Sevilla, 2014).

The implementation of the controller to stabilize the rotational movements of the quadrotor requires knowledge of the angular positions and speeds, these variables can be known through sensors or estimated through the model that describes the dynamics of the system. A methodology that allows fusing such data is the Kalman filter, this methodology has been used in countless areas of engineering since its proposal by Kalman (1960), particularly in the area of unmanned aerial vehicles; the main application is the estimation of the position and orientation of the aircraft (attitude) (You et al., 2020;. Bauer & Bokor, 2008; Loianno et al., 2016; Markley, 2003; Emran et al., 2015; Sanz et al., 2014; Sebesta & Boizot, 2013; Muñoz et al., 2013; Gośliński et al., 2013; Sarim et al., 2015; Tailanián et al., 2014; Amoozgar et al., 2013; Xiong & Zheng, 2015). Likewise, there are proposals for filtering signals in unmanned aircraft based on state observers, with the advantage of reducing the computational cost, due not processing the calculation at each moment the profit matrix or Kalman matrix (Escobedo et al., 2018; Macdonald et al., 2014; Hanley & Bretl, 2016; Lendek et al., 2011; Shi et al., 2018; Yu et al., 2015). Other proposals for filtering signals with a low computational cost include complementary filters (Noordin et al., 2018; Jung & Tsiotras, 2007; Euston et al., 2008; Yoo et al., 2001), however; these proposals are less efficient than observers or the Kalman filter.

In this work, the Euler-Lagrange model of a quadcopter is analyzed, such as proposed in (Beard & McLain, 2012; Garcia et al., 2006; Castillo et al., 2007; Carrillo et al., 2012; Raffo, 2007), but additionally incorporates the gyroscopic effects of the rotation of the helices, then approximations based on Taylor series are made, that simplify the model rotation dynamics which we called complete and allow us to represent the dynamics in a reduced non-linear model similar to the one proposed in (Bouabdallah & Siegwart, 2007; Falconi & Melchiorri, 2012; Elruby et al., 2012; de Jesus Rubio et al., 2014), additionally, a linear model is obtained.

Aircraft modeling

This section describes the model that defines the dynamic behavior of translation and rotation of the quadcopter. To develop this model, the Euler-Lagrange formalism is used.

Quadcopter speeds, forces and motions

Figure 1 shows the reference frames used for modeling, as well as the free-body diagram. The orientation of the aircraft is defined by the Euler angles: η=ϕ,θ,ψ, pitch, roll, and yaw, respectively.

Figure 1: a) Reference frames, b) free-body diagram 

The total angular velocity of the aircraft is a sum of the rotational speeds in each reference axis of the system {c}. Therefore, the total angular velocity of the quadrotor represented by (p, q, r) =(Ω), is seen in the axes (X,Y,Z) of the reference frame {a} and is expressed according to the following relation (in the region where the Euler angles are valid), (Beard & McLain, 2012; Garcia et al., 2006; Castillo et al., 2007; Carrillo et al., 2012; Raffo, 2007).

pqr=ϕ˙00+1000cosϕsinϕ0-sinϕcosϕ0θ˙0+1000cosϕsinϕ0-sinϕcosϕcosθ0-sinθ010sinθ0cosθ00ψ˙ (1)

pqr=10-sinθ0cosϕsinϕcosθ0-sinϕcosϕcosθϕ˙θ˙ψ˙ (2)

Ω=Tωcη˙ (3)

According to Figure1b, the thrust force seen in the reference frame {c} is defined:

FT=(fe1+fe2+fe3+fe4) (4)

The thrust force fei for each engine and the aerodynamic drag force τi , which opposes the torque Mi generated by the rotation of the propellers, are expressed respectively, as:

fei=keωi2 (5)

Ti=kaωi2 (6)

Where, ωi is the angular velocity of each engine, ka and ke are positive definite constants that depend on the density of the air, the radius of rotation, the area and the shape of the propeller blades, as well as other factors. In conditions where the angular speed ωi of the motor is constant or the rates of change of this speed are small, the aerodynamic drag torque is equivalent to the torque produced by the motor (Beard & McLain, 2012; Garcia et al., 2006).

The thrust force acts on the “z” axis of the reference frame {c}, so the lift force in the inertial reference frame is defined:

fI=cosψ-sinψ0sinψcosψ0001cosθ0sinθ010-sinθ0cosθ1000cosϕ-sinϕ0sinϕcosϕ00fe (7)

fI=fe(sinϕsinψ+cosϕcosψsinθ)fe(cosϕsinθsinψ-cosψsinϕ)fecosθcosϕ (8)

The rotational movements are proposed on the bisector of the aircraft arms, that is, a flight in the "x" configuration. Then the rotation of an angle Φ (pitch) , is caused by a difference in the forces, such that:

τϕ=dfe1+fe2-dfe3+fe4 (9)

The rotation of an angle θ (roll) , is achieved through a difference in the forces, which produces a torque:

τθ=dfe2+fe3-dfe1+fe4 (10)

To obtain a rotation of an angle ψ (yaw) it is achieved by a difference in the torques (T3 + T1) and (T2 + T4) which produces:

τψ=T3+T1-(T2+T4) (11)

The aerodynamic drag torque Ti is expressed as:

Ti=lfri=lkrfei (12)

Where fri is the force perpendicular to length l and kr relates fei and fri, so that equation (13) can be rewritten as:

τψ=lfr3+fr1-(fr2+fr4) (13)

Also, each engine-propeller pair is considered as a rigid disk rotating with a speed ωi, around the “z”, axis, of the reference frame {c}, so the following gyroscopic effects (τGϕ, τGθ, τGψ), acts in the inertial reference frame, opposing to the control inputs. Where J is the moment of inertia of the engine-propeller pair and ωT=ω2+ω4-ω1-ω3.

τGϕτGθτGψ=-10-sinθ0cosϕsinϕcosθ0-sinϕcosϕcosθϕ˙θ˙ψ˙×00JωT=-JωTθ˙cosϕ+ψ˙sinϕcosθ-JωTψ˙sinθ-ϕ˙0 (14)

The equations that define the relation of the total thrust force, the torques and the thrust forces of each engine, are given by equations (9), (10), (11), (12) and (13), then the forces in each engine as a function of the control inputs are:

Fe1Fe2Fe3Fe4=    14-14    14 14    14    14-14 14-14-14    14-14     14 14-1414τϕdτθdτψlkrFT=14dτϕ-14dτθ+14lkrτψ+14FT14dτϕ+14dτθ-14lkrτψ+14FT-14dτϕ+14dτθ+14lkrτψ+14FT-14dτϕ-14dτθ-14lkrτψ+14FT (15)

Translation and rotation model

Due to the rotational kinetic energy and the translational kinetic energy do not have dependent terms; it is possible to separate the translation and rotation equations (Beard & McLain, 2012; Garcia et al., 2006; Castillo et al., 2007; Carrillo et al., 2012; Raffo, 2007). According to the Euler-Lagrange equations, the translation model is stated as:

mx¨y¨z¨+mg001=fe(sinϕsinψ+cosϕcosψsinθ)fe(cosϕsinθsinψ-cosψsinϕ)fe(cosθcosϕ) (16)

Likewise, the dynamic model that describes the rotation of the quadcopter is given as:

Mηη¨+Cn, η˙η˙=τ+τG (17)

Where Cn, η˙ is the Coriolis matrix, which contains the centrifugal forces and gyroscopic effects associated with the vector η, Mη: represents the inertia matrix and τG is the gyroscopic effects associated with rotation of the propellers. Also, due to the symmetric structure in the aircraft, the inertia matrix takes the form (Beard & McLain, 2012; Garcia et al., 2006; Castillo et al., 2007; Carrillo et al., 2012; Raffo, 2007).

Mη=TωcTITωc=1000cosϕ-sinϕ-sinθcosθsinϕcosθcosϕIx000Iy000Iz10-sinθ0cosϕsinϕcosθ0-sinϕcosϕcosθ (18)

In turn, the Coriolis matrix is defined as (Castillo et al., 2007; Raffo, 2007):

Cn, η˙=c11c12c13c21c22c23c31c32c33 (19)

Where:

c11=0

c12=Iy-Izθ˙cosϕsinϕ+ψ˙sin2ϕcosθ+IZ-Iyψ˙cos2ϕcosθ-Ixψ˙cosθ

c13=(IZ-Iy)(ψ˙cosϕsinϕcos2θ)

c21=Iz-Iyθ˙cosϕsinϕ+ψ˙sin2ϕcosθ+Iy-Izψ˙cos2ϕcosθ+Ixψ˙cosθ

c22=(Iz-Iy)(ϕ˙cosϕsinϕ)

c23=-Ixψ˙sinθcosθ+Iyψ˙sin2ϕcosθsinθ+Izψ˙cos2ϕsinθcosθ

c31=(Iy-Iz)(ψ˙cos2θsinϕcosϕ) -Ixθ˙cosθ

c32=IZ-Iyθ˙cosϕsinϕsinθ+ϕ˙sin2ϕcosθ+Iy-Izϕ˙cos2ϕcosθ+Ixψ˙sinθcosθ-Iyψ˙sin2ϕcosθsinθ-Izψ˙cos2ϕsinθcosθ

c33=(Iy-Iz)(ϕ˙cosϕsinϕcos2θ-Iyθ˙sin2ϕcosθsinθ-Izθ˙cos2ϕcosθsinθ+Ixθ˙cosθsinθ)

Reductions and simplifications to the complete rotation model

In this section, two approaches to the rotation model are presented. Initially, a classical approximate linearization is approached, then, an approximation is presented to obtain a reduced non-linear model.

Linear model

To linearize the system, the rotation model is expressed in state variables and the zero equilibrium state is considered. The dynamics of rotation in state variables from (17) is expressed as:

η˙η¨=η˙-Mη-1Cn, η˙η˙+Mη-1τG+Mη-1τ (20)

The state vector and the equilibrium input vector, corresponding to:

x-=η-η-˙=00 (21)

Defining the incremental states and incremental inputs as:

xδ=x-x-uδ=u-u- (22)

Then the linearized system around an equilibrium point takes the form:

x˙δ=Axδ+Buδ (23)

Where:

A=f(x-,u-)x=xfx,u(x-,u-)

B=f(x-,u-)u=ufx,u(x-,u-)

Therefore, the approximate linear system is defined by the following expression:

η˙η¨=03x3I3x303x3Mbη-1τbGηη˙+03x3Mbη-1τ (24)

τbG=0-JωT0JωT00000,Mbη=Ix000Iy000Iz, η=ϕθψ,η˙=ϕ˙θ˙ψ˙

Simplifying the previous expressions, we obtain:

ϕ¨θ¨ψ¨=1Ix0001Iy0001Iz0-JωT0JωT00000ϕ˙θ˙ψ˙+1Ix0001Iy0001Izτϕτθτψ (25)

Reduced non-linear model

Another way to reduce the complexity of the model is to consider small movements, that is, the transformation matrix TωcI, in this way Ωη˙. Also, we can approximate:

cosϕcosθcosψ1;sinϕϕ,sinθθ,sinψψ (26)

Then, applying these approximations to the inertia matrix (18) and Coriolis matrix (19), we obtain:

Mcη=TωcTITωc=100010001Ix000Iy000Iz100010001=Ix000Iy000Iz (27)

Ccn, η˙=cc11cc12cc13cc21cc22cc23cc31cc32cc33cc11=0cc12=ψ˙Iz-Iy-Ix+θ˙ϕIy-Izcc13=ψ˙ϕIz-Iycc21=ψ˙(Iy+Ix-Iz)+θ˙ϕIz-Iycc22=ϕ˙ϕ(Iy-Iz)cc23=ψ˙θIz-Ixcc31=ψ˙ϕIy-Iz-θ˙Ixcc32=θ˙ϕθIz-Iy+ϕ˙Iy-Iz+ψ˙θIx-Izcc33=ϕ˙ϕ(Iy-Iz)+θ˙θIx-Iz (28)

τcG=τGϕτGθτGψ=-100010001ϕ˙θ˙ψ˙×00JωT=-θ˙JωTϕ˙JωT0 (29)

Then replacing (27), (28), (29) in (17) and rearranging, we obtain the reduced non-linear model for the rotation dynamics:

ϕ¨θ¨ψ¨=-1Ix0001Iy0001Izcc11cc12cc13cc21cc22cc23cc31cc32cc33ϕ˙θ˙ψ˙+1Ix0001Iy0001Iz-θ˙JωTϕ˙JωT0+1Ix0001Iy0001Izτϕτθτψ (30)

Computer torque controller

The open-loop dynamics of the quadcopter is unstable, so it is necessary to apply a closed-loop control system. In this section, we propose a control system based on the computed torque technique (Reyes, 2011; Sira et al., 2005).

Control based on the full model

The controller designed using the full model is:

τ=Mηη¨*-Kaη˙e-Kbηe-Ki0tηedt+Cn,η˙η˙-τG (31)

Where:

ηe=η-η*;η˙e=η˙-η˙* ; η¨e=η¨-η¨* the superscript (*) represents the desired value.

Introducing the control in (17) and simplifying is obtained:

η¨=-Mη-1Cn,η˙η˙+Mη-1τG+Mη-1τ+ξ (32)

η¨=-Mη-1Cn,η˙η˙+Mη-1τG+Mη-1Mηη¨*-Kaη˙e-Kbηe-Ki0tηδdt +C(n,η˙)η˙-τG+ξ (33)

The term ξ, are constant disturbances. Then the closed-loop dynamics that define the behavior of the quadcopter is:

η¨=η¨*-Kaη˙e-Kbηe-Ki0tηedt+ξ (34)

Ka=k2000k4000k6, kb=k1000k3000k5,ki=k7000k8000k9 (35)

ϕ¨eθ¨eψ¨e=-k1ϕe-k2ϕ˙e-k70tϕedt+ξϕ-k3θe-k4θ˙e-k80tθedt+ξθ-k5ψe-k6ψ˙e-k90tψedt+ξψ (36)

When each of the previous expressions is derived, we can observe that the perturbations considered constant become zero, then the closed-loop stability can be analyzed through the characteristic polynomial, choosing positive definite values for Ka, Kb and Ki, such that Ka, Kb > ki; the system is stable.

Control based on linear model

Following the same strategy applied to the complete model, the control designed through the linear system is:

τb=Mbηη¨*-Kaη˙e-Kbηe-Ki0tηedt-τbGη˙ (37)

τb=τϕτθτψ=Ixϕ¨*-k1ϕe-k2ϕ˙e-k70tϕedt+θ˙JωTIyθ¨*-k3θe-k4θ˙e-k80tθedt-ϕ˙JωTIzψ¨*-k5ψe-k6ψ˙e-k90tψedt (38)

Control based on reduced nonlinear model

In turn, the control system designed from the reduced system is:

τc=Mcηη¨*-Kaη˙e-Kbηe-Ki0tηedt-τcG+Ccn,η˙η˙ (39)

τc=τϕτθτψ=Ixϕ¨*-k1ϕe-k2ϕ˙e-k70tϕedt+θ˙JωTIyθ¨*-k3θe-k4θ˙e-k80tθedt-ϕ˙JωTIzψ¨*-k5ψe-k6ψ˙e-k90tψedt+Ccn,η˙ϕ˙θ˙ψ˙ (40)

Signal filtering

Figure 2a, shows the signal of one of the rotation angles of the quadcopter prototype, the signal is obtained from the projection of gravity on the axes of a low-cost Inertial Measurement Unit of 6 degrees of freedom (IMU 6 DOF, MPU6050), in addition, the angular positions and speeds of the drone can be accessed. Figure 2b shows one of the duty cycle signals (1000 us to 2000 us) with a frequency of 60 Hz, sent to the electronic speed controllers (ESC). From these graphs, we can observe the impact of mechanical vibration (noise) on the acquisition of the rotation signal, also, the noise increases as the speed of the motors increases.

Figure 2: a) Angle of rotation contaminated in noise, b) duty cycle of one of the engines 

For this reason, in this section, systems are developed and synthesized for filtering the quadcopter rotation signals.

Standard Kalman filter

The standard Kalman filter is an optimal state estimator and is applicable to a linear system, described by the following equation of state represented in discrete form; where: wk and vk are the process and measurement noise respectively.

xk+1= Axk+Buk+wk (41)

yk=Cxk+vk (42)

The equations that describe the standard Kalman filter are defined as (Kalman, 1960; Simon, 2006):

Kk=PkCTCPkCT+R-1 (43)

Pk+1=AI-KkCPkAT+Q (44)

x^k+1=Ax^k+Buk+Kkyk-Cx^k (45)

x^k=η^kη˙^k, η^k=ϕkθkψk,η˙^k=ϕ˙kθ˙kψ˙k, uk=τk (46)

Where:

x^k = vector of estimated states

yk = measurement matrix

Kk = filter gain matrix

Q = covariance of the noise process wk

R = covariance of the noise in the measurement vk

I = identity matrix

Also and according to the linear system (25), matrix A , matrix B , matrix C and the vector of states to be estimated are defined as:

A=03x3I3x303x3Mbηk-1τbG, B=03x3Mbηk-1, C=I3x303x303x3I3x3 (47)

Extended Kalman filter

The extended Kalman filter, unlike the standard KF, uses the non-linear model to estimate the system states. That is, the equation of state is represented as:

xk+1= fxk,uk+wk (48)

yk=hxk+vk (49)

The equations that define the extended Kalman filter are (Simon, 2006):

Kk=PkCkTCkPkCkT+R-1 (50)

Pk+1=AkI-KkCkPkAkT+Q (51)

x^k+1=fx^k,uk+Kkyk-hx^k (52)

The application of the previous equations requires the calculation of the matrices Ak and Ck, at each instant of time, these matrices are calculated trough the partial derivative of the equation of state and the output equation, then the results are evaluated in the vector of updated estimated states. According to the reduced nonlinear model (30), the matrix Ak and the matrix Ck are defined as:

Ak=f'x^k,uk=0       0    00       0    00       0    01        0     00        1    00        0    1Ak410 0  Ak51Ak520  Ak61Ak620  0Ak45Ak46Ak54Ak55Ak56Ak64Ak65Ak66 (53)

Where:

Ak41=1Ixθ˙k2Iy-Iz+1Ixψ˙k2-Iy+Iz Ak45=1Ixψ˙k-Ix-Iy+Iz+1Ix2θ˙kϕkIy-Iz-JωTIx
Ak46=1Ixθ˙k-Ix-Iy+Iz+1Ix2ψ˙kϕk-Iy+Iz Ak51=1Iy2ϕ˙kθ˙k-Iy+Iz
Ak52=1Iyψ˙k2-Ix+Iz Ak54=1Iyψ˙kIx+Iy-Iz+1Iy2θ˙kϕk-Iy+Iz+JωTIy
Ak55=1Iy2ϕ˙kϕk-Iy+Iz Ak56=1Iyϕ˙kIx+Iy-Iz+1Iy2ψ˙kθk-Ix+Iz
Ak61=1Izθ˙k2θk-Iy+Iz+1Iz2ϕ˙kψ˙kIy-Iz Ak62=1Izθ˙k2ϕk-Iy+Iz+1Iz2θ˙kψ˙kIx-Iz
Ak64=1Izθ˙k-Ix+Iy-Iz+1Iz2ψ˙kϕkIy-Iz Ak65=1Izϕ˙k-Ix+Iy-Iz+1Iz2θ˙kθkϕk-Iy+Iz+1Iz2ψ˙kθkIx-Iz
Ak66=1Izθ˙kθkIx-Iz+1Iz2ϕ˙kϕkIy-Iz

Ck=h'x^k=I3x303x303x3I3x3, x^k=η^kη˙^k, uk=τk (54)

Filter based on observer

The observability matrix of (A , C ), for the linear system (25), is full-range, so the state observer is established as:

x^˙=Ax^+Bu+Koy-y^ (55)

y^=Cx^,x^=η^η˙^ (56)

The equation that defines the dynamics of the error that relates to the estimated states and the real states is:

e˙o=x˙-x^˙=Ax+Bu-Ax^-Bu-KoCx-Cx^) (57)

e˙o=A-koCeo (58)

We can verify the stability of the system through the eigenvalues of the matrix (A - koC), where the values can be chosen for the ko, matrix, such that, the observer behaves in a stable dynamic.

Filter based on extended observer

We can estimate the linear representation of the dynamics of the aircraft through a states observer, so the estimate states can be extrapolated to the non-linear model, that is, an extended Luenberger-type observer is proposed, now the estimation of the states is given by:

x^˙=f(x,^u)+Key-y^ (59)

Now the dynamics of the error between the estimated states with the real states is defined as:

e˙=x˙-x^˙=fx,u-f(x,^u)+Keh(x)-h(x^) (60)

As the movements of the system are controlled around the equilibrium states x=0 and x˙=0, the dynamics of the error can be studied in a linear way, taking the first term of the decomposition by means of Taylor series, around the value e=0.

e˙=fx,u-f(x+e,u)+Keh(x)-h(x+e) (61)

e˙=δδxfx-,u-+Keδδxhx- e+0e2 (62)

e˙=A-keCe (63)

Simulation

In this section, numerical simulations in open-loop, closed-loop and filtering system are presented with the different models, the simulation was executed using MatLab software in the Simulink environment, the numerical method used was Runge-Kutta of fourth-order to a simulation step of 0.001s. The parameters that define the complete aircraft model, as well as the parameters used in the control and filtering system, are shown in Table 1.

Table 1: Model parameters 

Parameter Value in model Value in
controller and
filtering system
Unit
l 0.23 0.21 m
d 0.16 0.15 M
m 1.1 1.1 Kg
g 9.81 9.81 m/s2
Ix 15x10-3 45x10-3 Kg m2
Iy 15x10-3 45x10-3 Kg m2
Iz 30x10-3 90x10-3 Kg m2
J 1x10-4 3x10-4 Kg m2
kr 1/12 1/10
ke 1.5x10-6 1.5x10-6

Open loop simulation

In open-loop simulations, the initial conditions are established as: ϕ,θ,ψ=0.0, 0.0, 0.0rad and (ϕ˙,θ˙,ψ˙)=(0, 0, 0)rad/s, the following torques are proposed as constants; (τϕ,τθ,τψ)=(0.002, -0.002, 0.002)Nm. Figures 3a, 3b and 3c correspond to the comparison of the rotation models, we can observe that the models have similar behavior in a small region of operation, then in aggressive flight conditions, for example beyond 1 rad, the divergence of the linear model is evident.

Figure 3: Open-loop comparison of the behavior of each model, a) Pitch axis, b) roll axis, c) yaw axis 

Control system simulation

In the closed-loop simulation, each of the controllers designed through the different models is applied to the complete non-linear dynamics of the rotation system, equation (17).

To compare the response of the different controllers; the performance index is calculated under the L2 (Reyes, 2011) norm, equation (64). The initial conditions are established as: ϕ,θ,ψ=0.3, 0.3, 0.3rad and ϕ˙,θ˙,ψ˙=0.3, 0.3, 0.3 rad/s, the gains of the controllers, are detailed in Table 2.

L2=q~=  1T0Tq~(t)2dt (64)

Figure 4 corresponds to the comparison of the response of the different controllers in the simultaneous movements in each axis of the aircraft. The overlapping responses of each controller are observed, only with the performance index presented in 4d, we can distinguish that the control designed with the reduced model offers the best performance.

Figure 4: Response of the controllers acting on the complete model, a) Pitch angle, b) roll angle, c) yaw angle, d) performance index 

Simulation of the control system and filtering system

Figure 5 illustrates through a block diagram the methodology used in the simulation of the noise process and filtering system. Table 2 defines the controller and observer gains.

Figure 5: Block diagram of the simulation, control system and filtering system 

Table 2: Gains of the control system and filtering system 

Gains
Ka=300003000030 Ki=0.10000.10000.1

Kb=200002000020 ko=ke=100010001000000000000000000100010001

Figures 6b to 6d, show the behavior with the reduced model in the filtering and control system for simultaneous movements in the axes of rotation of the aircraft, when is adding the white noise signal to the states of the system defined in 6a. The behavior of the reduced model offers the best performance for the controller and the filtering system as seen in Figure 7a and 7b. To obtain and compare the performance index of the control system, the different models are applied to the control and the filtering stage, then to obtain the performance index of the filtering stage, we proceed similarly.

Figure 6: Response to the noise of the control system and filtering system with a reduced model, a) Noise added to the system states, b) pitch, c) roll, d) yaw 

Figure 7: a) Control performance with different models and filters, b) filtering performance with different models and filters 

We can see en Figure 7, that filter system with observers or Kalman filters is very similar, this can be explained because the operation of the aircraft is considered in a small region, even if there are combined movements in the rotations. Due to the implementation of the observer is simpler than the Kalman filter. Figure 8 shows the comparison of the controller and filtering through observers with the different models for combined or simultaneous movements in each axis of rotation.

Figure 8: Comparison of the controller and filtering using observers with the different models for combined or simultaneous movements in each axis of rotation, a) Pitch angle, b) roll angle, c) yaw angle 

It has shown that the performance offered by the control system and filtering system with the linear model is similar to that obtained with a Kalman filter or an observer using the full or reduced model. Due that it is simpler to implement a filtering and control system based on the linear model, we chose to implement the experimental part with this model.

Also, in order to compare the proposed control that bases its design on the approximate linear model of the aircraft with a control technique that does not require a dynamic model for its design, a comparison with a fuzzy logic controller is proposed.

Fuzzy logic control

The fuzzy-logic controller design is based, on producing changes in the control signal through the system error and its change reason, i.e. a PD system. The membership functions are presented in Figure 9. Where the following variables are defined for the error, its derivative, as well as the control or output signal; Negative Big (NB), Negative Small (NS), Zero (Z), Positive Small (PS) and Positive Big (PB).

Figure 9: Membership functions, a) Error, b) derived error, c) signal control 

The inference rules are directly related to the linguistic variables defined in the membership functions and are presented in Table 3. Also, for the controller design and the defuzzification signals, the Fuzzy Logic Design toolbox from Matlab is used, establishing Sugeno's methodology and centroid defuzzification.

Table 3: Inference rules 

e(t) / d e(t)/dt NB NS Z PS PB
NB NB NS NS NS Z
NS NS NS NS Z PS
Z NS NS Z PS PS
PS NS Z PS PS PS
PB Z PS PS PS PB

The comparison in a simulation of the proposed controller with the fuzzy controller is shown in Figure 10, as can be seen, any of the controllers achieves the objective of stabilizing the system with similar behavior, however, although the fuzzy controller offers adequate system behavior and not require a model for its design has the disadvantage; requires precise knowledge of the experimental behavior of the system to propose the membership functions and inference rules, likewise, the implementation in a microcontroller requires more computational resources for its execution, mainly in the defuzzification stage, therefore and due to that the behavior offered by the controller proposed as well as the relative simplicity with which it can be implemented in a microcontroller, it is chosen to implement the experimental part with this controller.

Figure 10: Comparison of the proposed controller and fuzzy-logic controller, a) Pitch angle, b) roll angle, c) yaw angle 

Now the next section of the work shows the experimental results of this implementation.

Microcontroller implementation and experimental results

The implementation is done according to the block diagram of Figure 11. The signals of the IMU are interpreted and filtered in the microcontroller, also the control signals are calculated using a linear model, the equations that define the linear observer are solved by the Runge Kutta numerical method at an operation step of 0.001s. The experimentation is divided into two stages. The first one, through a test bench that allows rotations in the aircraft's pitch, roll and yaw axes but restricts translational movements, the second stage in a quasi-static flight in which only the rotation movements are controlled around the origin and the translational movements of the aircraft are left free. Figure 12 shows both stages of experimentation.

Figure 11: Block diagram of the implementation 

Figure 12: Experimentation, a) test bench, b) and c) quasi-static flight 

Experimental results on the test bench

The experimental results of the implementation of the controller and filtering stage with the test bench are shown in Figure 13.

Figure 13: Results of the experimental stage on the test bench, a) Filtered signal for the pitch angle, b) filtered signal for the roll angle, c) trajectory tracking on the pitch axis, d) trajectory tracking on the roll axis, e) control signal for the pitch movement, f) control signal for the roll movement, g) yaw angle stabilization, h) control signal for the yaw movement 

Experimental results in quasi-static flight

The experimental results of the implementation of the controller and filtering stage in a quasi-static flight are shown in Figure 14.

Figure 14: Experimental results of the quasi-static flight stage. a) Signal filtering for pitch angle, b) signal filtering for roll angle, c) pitch axis stabilization, d) roll axis stabilization, e) control signal for pitch movement, f) control signal for roll movement, g) yaw angle stabilization, h) control signal for the yaw movement 

Conclusions

In this work, a dynamic model was developed using Euler-Lagrange for a quadcopter type aircraft, then a reduced model was obtained from the complete model, as well as a linear model. Afterward, its open-loop response was compared, observing that for an operating region less than 1 rad, the models behave similarly, later, controllers were designed using the computed torque technique based on the various models and a PID control, the controllers were compared through their performance index, it was determined that anyone controller achieves the objective of stabilizing or tracking the trajectory, later filtering systems were analyzed using observers and Kalman filters, where for each case the three analyzed models were applied, then it was compared the response of the controller and filtering system through a performance index when noise is added to the states and constant disturbances in the parameters, with this it was determined that a linear model is sufficient to design a controller and filtering system, which implies simple implementation and reduction of computational resources for operation with a microcontroller. Additionally, the proposed controller was compared in simulation with a fuzzy logic controller, it was observed that both controllers have similar behavior, however, the proposed controller is easy to implement in a microcontroller, and the stability of the system and filtering is justified with the mathematical analysis presented, against the fuzzy logic controller, for this reason, it was determined to implement the experimental part with said controller. Subsequently, the control and filtering system was implemented for a low-cost IMU and a 32-bit ARM microcontroller, the experimental results showed a behavior similar to the simulation, which validates the use of a linear model to stabilize and track trajectories in the operation of a non-aggressive or aerobatic flight of the aircraft. However, in the experimental part, it was noted that a linear model is sufficient to design the filter and controller, but a control strategy is necessary to give the system robustness to the disturbances caused by the wind or by the disturbances caused by no to know exactly the thrust force of each motor-propeller pair. It is also recommended to design an algorithm that allows improving the performance of the controller estimating the thrust forces of each motor.

References

Agho, C. (2017). Dynamic model and control of quadrotor in the presence of uncertainties. [ Links ]

Amoozgar, M. H., Chamseddine, A. & Zhang, Y. (2013). Experimental test of a two-stage Kalman filter for actuator fault detection and diagnosis of an unmanned quadrotor helicopter. Journal of Intelligent & Robotic Systems, 70(1-4), 107-117. [ Links ]

Balasubramanian E. & Vasantharaj R. (2013). Dynamic modeling and control of quadrotor. International Journal of Engineering and Technology (IJET), 5(1), 01-07. [ Links ]

Bauer, P. & Bokor, J. (2008). LQ Servo control design with Kalman filter for a quadrotor UAV. Periodica Polytechnica Transportation Engineering, 36(1-2), 9-14. [ Links ]

Beard, R. W. & McLain, T. W. (2012). Small unmanned aircraft: Theory and practice. Princeton university press. [ Links ]

Bouabdallah, S., Noth, A. & Siegwart, R. (2004). PID vs LQ control techniques applied to an indoor micro quadrotor. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)(IEEE Cat. No. 04CH37566) 3, 2451-2456. IEEE. [ Links ]

Cárdenas G. (2015). Drones Ciencia al Vuelo. ¿Cómo vez?, UNAM, México, 2015. [ Links ]

Carrillo, L. R. G., López, A. E. D., Lozano, R. & Pégard, C. (2012). Quad rotorcraft control: vision-based hovering and navigation. Springer Science & Business Media. [ Links ]

Castillo, P., García, P., Lozano, R. & Albertos, P. (2007). Modelado y estabilización de un helicóptero con cuatro rotores. Revista Iberoamericana de Automática e Informática Industrial RIAI, 4(1), 41-57. https://doi.org/10.1016/S1697-7912(07)70191-7. [ Links ]

De Jesús-Rubio, J., Cruz, J. H. P., Zamudio, Z. & Salinas, A. J. (2014). Comparison of two quadrotor dynamic models. IEEE Latin America Transactions, 12(4), 531-537. [ Links ]

Elruby, A. Y., El-Khatib, M. M., El-Amary, N. H. & modeling Hashad, A. I. (2012). Dynamic and control of quadrotor vehicle. In The International Conference on Applied Mechanics and Mechanical Engineering 15(5). International Conference on Applied Mechanics and Mechanical Engineering. 1-10. Military Technical College. [ Links ]

Emran, B. J., Al-Omari, M., Abdel-Hafez, M. F. & Jaradat, M. A. (2015). Hybrid low-cost approach for quadrotor attitude estimation. Journal of Computational and Nonlinear Dynamics, 10(3). https://doi.org/10.1115/1.4028524. [ Links ]

Escobedo, J. O., Garcia, E. C., Páramo, L. A., Meda, J. A. & Tapia, R. (2018). Theoretical application of a hybrid observer on altitude tracking of quadrotor losing GPS signal. IEEE Access, 6, 76900-76908. [ Links ]

Euston, M., Coote, P., Mahony, R., Kim, J. & Hamel, T. (2008). A complementary filter for attitude estimation of a fixed-wing UAV. IEEE/RSJ international conference on intelligent robots and systems 340-345. [ Links ]

Falconi, R. & Melchiorri, C. (2012). Dynamic model and control of an over-actuated quadrotor uav. IFAC Proceedings, 45(22), 192-197. [ Links ]

Floreano, D. & Wood, R. J. (2015). Science, technology and the future of small autonomous drones. Nature, 521(7553), 460-466. [ Links ]

Garcia, P. C., Lozano, R. & Dzul, A. E. (2006). Modelling and control of mini-flying machines. Springer Science & Business Media. [ Links ]

Gautam, D. & Ha, C. (2013). Control of a quadrotor using a smart self-tuning fuzzy PID controller. International Journal of Advanced Robotic Systems, 10(11), 380. [ Links ]

Gośliński, J., Giernacki, W. & Gardecki, S. (2013, June). Unscented Kalman Filter for an orientation module of a quadrotor mathematical model. 9th Asian Control Conference (ASCC) 1-6. IEEE. [ Links ]

Gupta, S. G., Ghonge, D. & Jawandhiya, P. M. (2013). Review of unmanned aircraft system (UAS). International Journal of Advanced Research in Computer Engineering & Technology (IJARCET), 2. [ Links ]

Gutiérrez, A. R. H., Duarte, M. A. T. & France, R. G. (2017). El uso de drones en ciencias de la tierra. [ Links ]

Hanley, D. J. & Bretl, T. W. (2016). An improved model-based observer for inertial navigation for quadrotors with low cost imus. In AIAA Guidance, Navigation, and Control Conference 0105. [ Links ]

Hossain R., Rideout D. & Krouglicof N. (2010). Bond graph dynamic modeling and stabilization of a quadrotor helicopter. ResearchGate. [ Links ]

Jung, D. & Tsiotras, P. (2007). Inertial attitude and position reference system development for a small UAV. In AIAA Infotech@ Aerospace 2007 Conference and Exhibit 2763. [ Links ]

Kalman, R. E. (1960). A new approach to linear filtering and prediction problems. [ Links ]

Lendek, Z., Berna, A., Guzmán-Giménez, J., Sala, A. & García, P. (2011). Application of Takagi-Sugeno observers for state estimation in a quadrotor. 50th IEEE Conference on Decision and Control and European Control Conference 7530-7535. IEEE. [ Links ]

Loianno, G., Brunner, C., McGrath, G. & Kumar, V. (2016). Estimation, control, and planning for aggressive flight with a small quadrotor with a single camera and IMU. IEEE Robotics and Automation Letters, 2(2), 404-411. [ Links ]

Macdonald, J., Leishman, R., Beard, R. & McLain, T. (2014). Analysis of an improved IMU-based observer for multirotor helicopters. Journal of Intelligent & Robotic Systems , 74(3-4), 1049-1061. [ Links ]

Markley, F. L. (2003). Attitude error representations for Kalman filtering. Journal of guidance, control, and dynamics, 26(2), 311-317. https://doi.org/10.2514/2.5048. [ Links ]

Muñoz, L. E., Castillo, P. & García, P. (2013). Observer-control scheme for autonomous navigation: Flight tests validation in a quadrotor vehicle. International Conference on Unmanned Aircraft Systems (ICUAS) 95-804. IEEE. [ Links ]

Nonami, K., Kendoul, F., Suzuki, S., Wang, W. & Nakazawa, D. (2010). Introduction. In Autonomous Flying Robots. Japan: Springer. 1-29. [ Links ]

Noordin, A., Basri, M. A. M. & Mohamed, Z. (2018). Sensor fusion algorithm by complementary filter for attitude estimation of quadrotor with low-cost IMU. Telkomnika, 16(2), 868-875. [ Links ]

Paiva-Peredo, E. A. (2016). Modelado y control de un cuadricóptero. [ Links ]

Patete, A. & Erazo, A. (2016). Linear and nonlinear control for a micro unmanned aerial vehicle with quadrotor configuration. Revista Ingenierıa UC, 23(3), 247-259. [ Links ]

Raffo, G. V. (2007). Modelado y Control de un Helicóptero Quad-Rotor. MS, disertación, Dept. telemática, automática y robótica, Univ. de Sevilla. https://www.researchgate.net/deref/http%3A%2F%2Fdx.doi.org%2F10.1109%2FIROS.2007.4399042. [ Links ]

Reyes, F. (2011). Robótica-control de robots manipuladores. Alfaomega grupo editor. [ Links ]

Roldan-Caballero A. (2016). Modelado y monitoreo para un drone tipo cuadrirotor (Tesis de Maestría en Ingeniería de Manufactura). Instituto Politécnico Nacional. Ciudad de México. [ Links ]

Rodríguez R., Ipanaqué W., Carrera E. (2016).Comparación de modelos matemáticos y controladores PID vs LQR para un cuadricóptero. Universidad de Piura, Facultad de Ingeniería. [ Links ]

Sabatino, F. (2015). Quadrotor control: modeling, nonlinearcontrol design, and simulation. [ Links ]

Sadr, S., Moosavian, S. A. A. & Zarafshan, P. (2014). Dynamics modeling and control of a quadrotor with swing load. Journal of Robotics, 2014. https://doi.org/10.1155/2014/265897. [ Links ]

Sanz, R., Ródenas, L., García, P. & Castillo, P. (2014). Improving attitude estimation using inertial sensors for quadrotor control systems. International Conference on Unmanned Aircraft Systems (ICUAS) 895-901. IEEE. [ Links ]

Sarim, M., Nemati, A., Kumar, M. & Cohen, K. (2015). Extended kalman filter based quadrotor state estimation based on asynchronous multisensor data. In Dynamic Systems and Control Conference 57243. American Society of Mechanical Engineers. https://doi.org/10.1115/DSCC2015-9929. [ Links ]

Sebesta, K. D. & Boizot, N. (2013). A real-time adaptive high-gain EKF, applied to a quadcopter inertial navigation system. IEEE Transactions on Industrial Electronics, 61(1), 495-503. [ Links ]

Sevilla, L. (2014). Modelado y control de un cuadricóptero. Universidad Pontificia de Comillas. [ Links ]

Shi, D., Wu, Z. & Chou, W. (2018). Generalized extended state observer based high precision attitude control of quadrotor vehicles subject to wind disturbance. IEEE Access , 6, 32349-32359. [ Links ]

Simon, D. (2006). Using nonlinear Kalman filtering to estimate signals. Embedded Systems Design, 19(7), 38. [ Links ]

Sira-Ramírez, H. E. B. E. R. T. T., Márquez, R., Rivas-Echeverria, F. & Llanes-Santiago, O. (2005). Control de Sistemas no lineales. Automática & Robótica. Pearson Educación SA. [ Links ]

Tailanián, M., Paternain, S., Rosa, R. & Canetti, R. (2014). Design and implementation of sensor data fusion for an autonomous quadrotor. IEEE International Instrumentation and Measurement Technology Conference (I2MTC) Proceedings 1431-1436. IEEE. [ Links ]

Xiong, J. J. & Zheng, E. H. (2015). Optimal kalman filter for state estimation of a quadrotor UAV. Optik, 126(21), 2862-2868. [ Links ]

You, W., Li, F., Liao, L. & Huang, M. (2020). Data Fusion of UWB and IMU Based on Unscented Kalman Filter for Indoor Localization of Quadrotor UAV. IEEE Access , 8, 64971-64981. [ Links ]

Yu, J., Shi, P., Dong, W. & Yu, H. (2015). Observer and command-filter-based adaptive fuzzy output feedback control of uncertain nonlinear systems. IEEE Transactions on Industrial Electronics , 62(9), 5962-5970. [ Links ]

Yoo, T. S., Hong, S. K., Yoon, H. M. & Park, S. (2011). Gain-scheduled complementary filter design for a MEMS based attitude and heading reference system. Sensors, 11(4), 3816-3830. [ Links ]

Annexed

Glossary

ϕ: is pitch angle.

θ: is roll angle.

ψ: is yaw angle.

{c}: is the reference axis of the system.

{a}: is frame with same orientation of inertial frame.

Ω: is the total angular velocity of the quadrotor.

pqr: are the state of the vector Ω.

FT: is the thrust force of the aircraft.

fei: is the aerodynamic drag force.

ωi: is the angular velocity of each engine.

ka, ke: are positive definite constants that depend on the density of the air, the radius of rotation, the area and the shape of the propeller blades, as well as other factors.

fI: is defined like to the lift force in the inertial reference frame.

J: is the moment of inertia of the engine-propeller pair.

τGϕτGθτGψ: are gyroscopic effects.

Fe1Fe2Fe3Fe4: these are the forces in each engine.

Cn, η˙: is definded as the Coriolis matrix.

Mη: represents the inertia matrix.

τG: is the gyroscopic effects associated with rotation of the propellers.

η˙η¨: is the dynamics of rotation in state variables.

x-: is the state vector and the equilibrium input vector.

xδ: is defined as the incremental states.

uδ: is defined as the incremental input.

x˙δ: is the linearized system around an equilibrium point.

wk, vk: are the process and measurement noise respectively.

x^k: is the vector of estimated states.

yk: is the measurement matrix.

Kk: is the filter gain matrix.

Q: is the covariance of the noise process wk.

R: is the covariance of the noise in the measurement vk.

I: is the identity matrix.

Received: September 16, 2020; Accepted: February 16, 2021

Creative Commons License This is an open-access article distributed under the terms of the Creative Commons Attribution License