software in the loop simulation for multirotor … · igor afonso acampora prado , davi ferreira de...

SOFTWARE IN THE LOOP SIMULATION FOR MULTIROTOR POSITIONTRACKING USING A LINEAR CONSTRAINED MODEL PREDICTIVE

CONTROLLER

Igor Afonso Acampora Prado∗, Davi Ferreira de Castro∗, Pedro Filizola Sousa MaiaGoncalves∗, Davi Antonio dos Santos∗

∗Praca Marechal Eduardo Gomes, 50, Vila das Acacias, 12228-900Instituto Tecnologico de Aeronautica

Sao Jose dos Campos, Sao Paulo, Brasil

Emails: igorap@ita.br, davifc@ita.br, pedrofsm@ita.br, davists@ita.br

Abstract— The multirotor unmanned aerial vehicles (UAVs) are experiencing a currently growing interestdue to their high maneuverability, simplified mechanics and vertical takeoff and landing capability. Also for thesereasons, the multirotor UAVs are suitable test-bed platforms for different position tracking control techniques.This paper solves the problem of safely controlling the position tracking of a multirotor UAV by using a linearstate-space model predictive controller (MPC) formulation. The optimization is performed by replacing theoriginal conic constraint set by a circumscribed pyramidal space that renders a linear set of inequalities onthe total thrust vector. In order to improve the design efficiency of such kind of control systems, the presentwork uses a low-cost software-in-the-loop (SIL) simulation scheme for performance evaluation of control law forquadricopter. The scheme consists of two computers interconnected by an Ethernet network using the UserDatagram Protocol (UDP). Basically, one computer runs MATLAB/Simulink while the other runs X-Plane.The control law is implemented in Simulink, whereas the vehicle dynamics as well as the flight environment aresimulated in X-Plane.

Keywords— Multirotor, Position Tracking, Model Predictive Control, Software-In-The-Loop.

Resumo— Os veıculos aereos nao tripulados (VANTs) do tipo multirrotor estao esfrentando um momento decrescente interesse devido a sua alta manobrabilidade, mecanica simplificada e capacidade de decolagem e pousouvertical. Tambem por essas razoes, os multirrotores sao plataformas de testes adequadas para diferentes tecnicasde controle de rastreamento de posicao. Este artigo resolve o problema de controlar o rastreamento de posicaode um multirrotor com seguranca, utilizando controle preditivo baseado em modedo linear em espaco de estados(MPC). A otimizacao e realizada substituindo o conjunto de restricoes do tipo conica original por um espaco derestricoes piramidal circunscrito que projeta um conjunto de inequacoes lineares sobre o vetor total de impuxo.A fim de melhorar a eficiencia de projeto de tal tipo de sistema de controle, o presente trabalho usa um esquemede simulacao de baixo custo Software-In-the-Loop (SIL) para avaliacao de desempenho da lei de controle de umquadricoptero. O esquema consiste de dois computadores interconectados por uma rede ethernet usando UserDatagram Protocol (UDP). Basicamente, um computador executa MATLAB/Simulink enquanto o outro executao X-Plane. A lei de controle e implementada em Simulink, enquanto a dinamica do veıculo e o ambiente de voo,sao simulados em X-Plane.

Palavras-chave— Multirrotor, Rastreamento de Posicao, Controle Preditivo Baseado em Modelo, Software-In-The-Loop.

1 Introduction

The multirotor-type unmanned aerial vehicle(UAV) technology has attracted a great deal ofinterest of the academia and industry and, con-sequently experienced a rapid improvement. Thisinterest is justified by features such as their sim-plified mechanics, low cost, high maneuverability,and vertical take-off and landing (VTOL) capa-bility. Multirotor UAVs has the potential to beemployed in applications too risky to human be-ings or simply intended to increase the efficiencyof certain tasks. Examples of applications arebuilding exploration, traffic monitoring, mappingof agricultural areas, search and rescue (Hoffmanet al., 2008).

The multirotors have six degrees of freedom(DOF): three of translational motion and threeothers corresponding to the rotation. However,they feature only four independent control vari-ables: three torque components and the magni-

tude of the total thrust vector. Therefore, the firstdifficulty in designing a control system for themcould seem to be the need for facing this under-actuation characteristic. However, it is necessaryto consider that in most of the applications with amarket potential, one is in fact concerned to con-trol only four DOFs: the three-dimensional posi-tion and the heading angle. The other DOFs mustonly to be stabilized. A popular control systemstructure that obviates the dynamics underactu-ation and allows to control the aforementionedfour DOFs is illustrated in Figura 1. The maingoal of this scheme is make the true vehicle posi-tion r ∈ R3 track some desired position commandrd ∈ R3. The navigation system is responsible forproviding estimated values, in engineering units,of the vehicle attitude d ∈ R3, angular velocityω ∈ R3, position r and linear velocity r ∈ R3. Thesystem structure is divided in two control loops,where the inner loop is responsible for controllingthe vehicle attitude and the outer loop carries out

the trajectory control by providing references ofattitude commands dref ∈ R3, seeing that the air-craft needs to perform an inclination, in relationto the rotor’s plane, to accelerate in a desired di-rection. Moreover, the outer loop computes thetotal thrust magnitude f ∈ R to control the verti-cal acceleration. On the other hand, the attitudecontrol block calculates the torque τ ∈ R3 thatmakes the aircraft follow the attitude commandsdref . Simulations reduce the development time of

Attitude

Control

Multirotor

Dynamics

Navigation

System

Trajectory

Control

�, ��

�

���

�

Sensors

�

�, �

Inner loop Outer loop

Figure 1: Block diagram structure.

the project, as it allows the testing more quicklyand at a lower cost, thus it is possible to com-pare different control laws in the literature. Forthe development of control systems, a great choiceis the software MATLAB/Simulink R©, but is needthe mathematical model of the vehicle to performthe simulation. These mathematical models arein some cases much as difficult to be generatedor not available (Benini et al., 2011). Further-more, it is difficult to model mathematically com-plex robot behaviours in realistic environments.To solve this problem it is utilized a commerciallyavailable flight simulator that is able to simulatethe vehicle dynamics as well as the flight environ-ment.

This paper refers to a scheme SIL simula-tion using MATLAB/Simulink R© and X-Plane R©to face the position tracking problem under con-straints on the total thrust vector. The corre-sponding constraint set is a piece of a cone withan inferior and a superior spherical lids. Thisproblem is solved using a linear state-space MPC,whose optimization is made handy by replacingthe original conic constraint set by a circum-scribed pyramidal space that renders a linear setof inequalities on the total thrust vector. The pa-per is organized as follows. Section II describesthe multirotor translational dynamic and definesthe position tracking problem and its solution.Section III describes Software-In-The-Loop sim-ulation scheme. Section IV simulations tests andSection V contains the conclusion and suggestionsfor future work.

2 Tracking Position Problem andSolution

This section presents the tracking position prob-lem and its solution taking into account linear con-

straints set by using MPC.

2.1 Translational dynamic model

Consider the multirotor vehicle and the threeCartesian coordinate systems (CCS) illustrated inFigure 2. Assume that the vehicle has a rigidstructure. The body CCS SB , {XB, YB, ZB}is fixed to the structure and its origin coincideswith the center of mass (CM) of the vehicle. Thereference CCS SR , {XR, YR, ZR} is Earth-fixedand its origin is at point O. Finally, the CCSSR′ , {XR′ , YR′ , ZR′} is defined to be parallel toSR, but its origin is shifted to CM. Assume thatSR is an inertial frame.

Figure 2: The Cartesian coordinate systems.

Invoking the second Newton’s law and ne-glecting disturbance forces, the translational dy-namics of the multirotor illustrated in Figure 2 canbe immediately described in SR by the followingsecond order differential equation:

r =1

mf +

00−g

, (1)

where r , [rx ry rz]T ∈ R3 is the position vector

of CM, f , [fx fy fz]T ∈ R3 is the total thrust

vector, m is the mass of the vehicle, and g is thegravitational acceleration. As illustrated in Fig-ure 2, f is perpendicular to the rotor plane.

Define the inclination angle φ ∈ R of the rotorplane to be the angle between ZB and ZR′ . Theangle φ can thus be expressed by

φ , cos−1fzf, (2)

where f , ‖f‖.Define the position tracking error r ∈ R3 as

r , r− rd, (3)

where rd , [rd,x rd,y rd,z]T ∈ R3 is a position

command.

Problem 1. Let φmax ∈ R denote the max-imum allowable value of φ and fmin ∈ R and

fmax ∈ R denote, respectively, the minimum andmaximum admissible values of f . The problem isto find a control law for f that minimizes r, sub-ject to the inclination constraint φ ≤ φmax and tothe force magnitude constraint fmin ≤ f ≤ fmax.

2.2 State-Space Model

Define the state vector x , [rx rx ry ry rz rz]T ∈

R6 and the control input vector u ∈ R3

u ,1

mf−

00g

. (4)

Let x(k) ∈ R6, u(k) ∈ R3 and y(k) ∈ R3

denote, respectively, the state vector, the controlinput vector and the controlled output vector alldescribed in discrete-time domain. Using the Eu-ler discretization method with a sample time ofTs = 30 ms, the discrete-time linear state-spacemodel is given as

x(k + 1) = Adx(k) + Bdu(k)y(k) = Cdx(k)

, (5)

where

Ad =

1 0.05 0 0 0 00 1 0 0 0 00 0 1 0.05 0 00 0 0 1 0 00 0 0 0 1 0.050 0 0 0 0 1

∈ R6×6,

(6)

Bd =

0.0013 0 00.05 0 0

0 0.0013 00 0.05 00 0 0.00130 0 0.05

∈ R6×3, (7)

and

Cd =

1 0 0 0 0 00 0 1 0 0 00 0 0 0 0 1

∈ R3×6 (8)

2.3 Thrust Vector Constraints

Using (4), the thrust magnitude constraint fmin ≤f ≤ fmax can be rewritten in terms of u as

fmin ≤

∥∥∥∥∥∥mu +m

00g

∥∥∥∥∥∥ ≤ fmax, (9)

fmin ≤ m√u2x + u2y + (uz + g)

2 ≤ fmax. (10)

Assuming that 0 ≤ φmax < π/2 rad, the incli-nation constraint φ ≤ φmax can be replaced by

cosφ ≥ cosφmax. (11)

(a)

intermediate

perimeter

(b)

Figure 3: (a) Real constraint space; (b) Linearizedconstraint space.

Using (2), (11) can be rewritten as

fzf≥ cosφmax, (12)

which in turn, using (4), can be rewritten in termsof u, yielding

uz + g√u2x + u2y + (uz + g)

2≥ cosφmax. (13)

In short, (10) and (13) give the nonlinear con-straints expressed in terms of the components ofthe control vector u. From the original form ofthe constraints as defined in Problem 1, one canvisualize them as a conic space with spherical in-ferior and superior lids, as illustrated in Figure3a. Note that, besides nonlinear, the original con-straint space is non-convex.

2.3.1 Linear Constraints

A linear suboptimal constraint space is now ob-tained as being the pyramidal space circumscribedin the original constraint space of Figure 3a. Thenew space is depicted in Figure 3b.

From Figure 3b, the new constraint along theZR′ axis can immediately expressed as

fmin ≤ fz ≤ fmax cosφmax. (14)

Consider an arbitrary fz and let it representthe ZR′ axis coordinate of the dotted perimeterrepresented in Figure 3b. The corresponding pro-jection on the XR′−YR′ plane is given in Figure 4.The later figure defines the distances α and β andenables to expresses them immediately as

α = 2β cosπ

4, (15)

andβ = fz tanφmax. (16)

𝑋R′

𝑌R′

𝛼

CM 𝑓x

𝑓y

𝛽

𝚷

𝟒

intermediate

perimeter

𝛼

Figure 4: Constraints top view.

Now, replace (16) into (15) to obtain

α =√

2fz tanφmax (17)

and note that the linear constraints along the hor-izontal axes are given by −α/2 ≤ fx, fy ≤ α/2. Tofinally obtain in matrix form yields

Λ f ≤ λ, (18)

where

Λ =

−1 0 −√22 tanφmax

1 0 −√22 tanφmax

0 −1 −√22 tanφmax

0 1 −√22 tanφmax

0 0 −10 0 1

(19)

and

λ =

0000

−fmin

fmax cosφmax

. (20)

Finally, using (4), (18) can be immediatelyreformulate in terms of u as

Λu ≤ λ, (21)

where

Λ , mΛ (22)

and

λ , λ−mΛ

00g

. (23)

2.4 Model Predictive Controller

This paper use the incremental-input formulation.The MPC is responsible for solving a quadraticprogramming by minimizing a quadratic cost

function subject to the aforementioned linear con-straints set. Only the first element of the op-timized control increments sequence is utilized.The control command that is effectively appliedon the plant is obtained by summing the last con-trol input with the optimized control incrementssequence The optimization process is repeated atthe next sampling instant based on new sensorsmeasurements. The mathematical formulation isfound with more details in (Lopes et al., 2011).

2.5 Computing Thrust Magnitude and AttitudeCommands

After the optimization process of the control in-put, it is necessary to convert u to the total thrustf and the attitude reference angles, which will beutilized as a set-point for some inner attitude con-trol loop. Thus, rewriting (4) as fx

fyfz

= m

uxuy

uz + g

, (24)

Then, by taking the norm of (24), we can de-fine f as

f = m√u2x + u2y + (uz + g)2. (25)

The attitude commands for the inner controlloop need to be computed from the vector f. How-ever, it is can be easily seen that there are infiniteattitude of SB to represent the multirotor orien-tation in SR, taking in account when the ZB axiscoincides with f. Therefore, it is necessary to spec-ify an unique attitude to the aerial robot. It canbe done utilizing the attitude represented by theprincipal Euler angle/axis (φ, e). Define the prin-cipal Euler axis e ∈ R3, shown in Figure (5), as

e =ZR′ × fxy‖ZR′ × fxy‖

, (26)

where fxy , [fx fy]T ∈ R2 denotes the horizontalprojection of f.

Utilizing the (2) and (26), it is possible to rep-resent the attitude of SB in relation to SR utiliz-ing, for example, Euler angles one obtains φ, θ, ψ.

3 SOFTWARE-IN-THE-LOOPSIMULATION SCHEME

The developed scheme provides resources for theevaluation of a trajectory control using a linearconstrained MPC applied to multirotors. Theschematic simulation contains computers withcomputing power and graphics to process calcu-lations for the MPC.

MATLAB/Simulink

X - Plane

Switch

Figure 5: Configuration of network computers.

3.1 Configuration scheme simulation

The scheme consists of two computers connectedby a network of Ethernet communication, asshown in Figure 5, where the first computerruns software MATLAB/Simulink R© and the sec-ond X-Plane R©. Control law is implemented inMATLAB/Simulink R©, while the dynamics andkinematics of the vehicle and the flight environ-ment are simulated in X-Plane R©.

3.2 Simulator X-Plane

For the development of this work was chosen sim-ulator X-Plane R© to implement the scheme simu-lation due to following characteristics: is certifiedby FAA (Federal Aviation Administration), it ispossible to simulate real conditions as the windand turbulence, the X-Plane R© provides a mod-ule for the construction of aircraft called Plane-Marker R©, Simulation-based blade elements andease in insertion and extraction of data fromthe simulator (Indriyanto and Jenie, 2010). Inpractice, the flight model based on ”Blade Ele-ment Theory”aerodynamic surfaces of the aircraft(wings, tail surfaces etc.) are split into severalparts, the force acting on each of them is calcu-lated by applying a fluid (Benini et al., 2011).

3.3 X-Plane R© interfacing withMATLAB/Simulink R©

The standard method of X-Plane R© to commu-nicate with external processes and machines isthrough User Datagram Protocol (UDP). Thefirst step to realize the communication betweenMATLAB/Simulink R© and X-Plane R© is to definewhat data will be sent from the X-Plane R© for di-agrams in MATLAB/Simulink R©. Data receivedby the MATLAB/Simulink R© are used to calcu-late controls, where inputs and outputs data ofX-Plane R© are illustrated in Figure 6.

Where T1, T2, T3 and T4 are respec-tively throttle commands input into the simulatedquadricopter in X-Plane. With relation to thedata output of the X-Plane R© they are definedas follows: rx, ry and rz are the linear positions;rx, ry and ry are the linear velocities; φ, θ and ψ

are the Euler angles and P, Q e R are the angularvelocities.

The sequence of the sending and receivingdata by the softwares MATLAB/Simulink R© andX-Plane R©, can be observed in Figure 3, whichappears three-phase flow data.

𝑇1

𝑇2

𝑇3

𝑇4

CONTROL

SYSTEM

Phase 1 Phase 3

Phase 2

MATLAB/Simulink®

X - Plane®

𝑓2

𝑓1

𝑓4

𝑓3

𝑟𝑦

𝑟𝑧

𝑟 𝑥

𝑟 𝑦

𝑟 𝑧

𝑟𝑥

ϕ

θ

ψ

P

Q

R

Figure 6: Data flow between X-Plane R© andMATLAB/Simulink R© control system.

Phase 1: The X-Plane R© sends the sensor datavia UDP to the aircraft control system imple-mented in MATLAB/Simulink R©. Phase 2: TheMATLAB/Simulink R© receives the data and per-forms the calculation of the control law which isthen sent to the X-Plane R©. Phase 3: The X-Plane R© receives control law data via UDP withvalues for throttle command of the simulated air-craft.

For more details about the communication be-tween MATLAB/Simulink R© and X-Plane R© work(Figueiredo and Saotome, 2012b) explains withgreater resources.

The aircraft model used for simulation wasdeveloped on the work of (Figueiredo and Sao-tome, 2012a)

4 Simulations and Results

All simulations were performed using The X-Plane R© and MATLAB/Simulink R©. To solve thequadratic programming was utilized the quadprogfunction in Matlab’s Optimization ToolBox. Thevehicle has mass of m =1 kg. In the whole simula-tion the MPC parameters were set asN = 70 (pre-diction horizon), M = 5 (control horizon), φmax =1 deg, fmax = 12 N, fmin = 2N µ = [1 1 1]T (out-put weightings) and ρ = [0.7 0.7 0.7]T (controlinput weightings). The vehicle attitude was stabi-lized by using a PD controller with Kp = 0.5 andKd = 0.009. In order to verify the behavior of thevehicle, a straight position was used as set-point tothe aircraft. As shown in Figure 9, the controllerwas able to make the vehicle follow the referencecommand with altitude overshoot Mp = 5%. Fig-

0 20 40 60 80 100-20

0

20

40

XR

[m

]

0 20 40 60 80 100-20

0

20

40

YR

[m

]

0 20 40 60 80 100

0

2

4

6

ZR

[m

]

Time [s]

rd,x

rx

rd,y

ry

rd,z

rz

Figure 7: φ and f .

ure 10 shows that in the beginning of the simu-lation the total thrust magnitude was saturatedon its upper bound to make the vehicle take offand then stabilized in approximately 9.81N thatrepresents the minimum thrust force to maintainthe vehicle on a fixed altitude. Furthermore, thelimits to φ were respected even with an oscillatoryresponse.

0 20 40 60 80 1000

5

10

15

f [N

]

Total Thrust Magnitude

0 20 40 60 80 100-0.5

0

0.5

1

1.5

phi [d

eg]

Time [s]

Principal Euler Angle

Figure 8: φ and f .

5 Conclusions

This paper presents a scheme for Software-In-the-Loop simulation applied to evaluate the per-formance of position tracking using MPC. Thescheme is the integration of two software’s. Asoftware responsible for plant simulation and the

other to simulate the control system. A strategywas adopted in which the vehicle takes off andfollows a straight position. With the predictivecontrol law that was tested in the environmentSIL, it has become possible to evaluate the controllaw tracking position, a more realistic and can beviewed movements of the aircraft in a flight sim-ulator. As future work can expand the positiontracking of a single vehicle for multiple vehicles,resulting in a cooperative control. Furthermore,one can carry out an expansion for a simulationHardware-in-the-Loop (HIL).

Acknowledgments

The authors acknowledge the financial support ofFAPEAM - Foundation for Research of the Ama-zon by means of Master Scholarships, as well asthe ITA - Aeronautical Institute of Technology forthe support needed to carry out this work.

References

Benini, A., Mancini, A., Minutolo, R., Longhi, S.and Montanari, M. (2011). A modular frame-work for fast prototyping of cooperative un-manned aerial vehicle, Journal of Intelligentand Robotic Systems 65: 507–520.

Figueiredo, H. V. and Saotome, O. (2012a). Mod-elagem e simulacao de veıculo aereo nao trip-ulado (vant) do tipo quadricoptero usando osimulador x-plane e simulink, Anais do XIXCongresso Brasileiro de Automatica, CBA2012, Campina Grande, Brazil.

Figueiredo, H. V. and Saotome, O. (2012b). Simu-lation platform for quadricopter: Using mat-lab/simulink and x-plane, XIV Simposio de

Aplicacoes Operacionais em Areas de Defesa,SIGE 2012, Sao Jose dos Campos, Brazil.

Hoffman, G. M., Waslander, S. L. and Tomlin,C. J. (2008). Quadrotor helicopter trajectorytracking control, Proceedings of the AIAAGuidance, Navigation and Control Confer-ence and Exhibit, Honolulu, Hawaii.

Indriyanto, T. and Jenie, Y. I. (2010). Model-ing and simulation of a ducted fan unmannedaerial vehicle ( uav ) using x-plane simulationsoftware, Regional Conference on Mechanicaland Aerospace Technology, Bali.

Lopes, R. V., Santana, P. H. R. Q. A., Borges,G. A. and Ishihara, J. Y. (2011). Modelpredictive controle applied to tracking andattitude stabilization of a vtol quadrotoraircraft, Proceedings of the 21st Interna-tional Congress of Mechanical Engineering -COBEM, Natal, Brazil.