model predictive control for uavs
TRANSCRIPT
Department of the Electronic Engineering
Master of Science in Electronic Engineering with emphasis in Automatic Control
Results of the master thesis:
Control and Trajectory generation for UAVs
By:Guillermo Julio César Betancourt Vera
Thesis Supervisor:Professor: Carlos Daniel García Beltrán
1. Dynamical model of quadcopterThe model is obtained by representing the aircraft as a rigid body evolving in a 3−D space
due to the main thrust and three torques. The dynamics of the four electric motors are fast and,thus, are neglected.The generalized coordinates of the rotorcraft are
q = (x, y, z, ψ, θ, φ) ∈ R6 (1.1)
where ξ = (x, y, z) ∈ R3 denotes the position of the center of mass of the quadcopter relative toa fixed inertial frame I, and η = (ψ, θ, φ) ∈ R3 are the Euler angles, ψ is the yaw angle aroundthe z − axis, θ is the pitch angle around the modified y − axis, and φ is the roll angle aroundthe modified x− axis, which represent the orientation of the quadcopter.Define the Lagrangian
L(q, q) = Ttrans + Trot − U, (1.2)where Ttrans = (m/2)ξT ξ is the translational kinetic energy, Trot = (1/2)ωTIω is the rotationalkinetic energy, U = mgz is the mass of the quadcopter, ω is the angular velocity, I is the inertiamatrix, and g is the acceleration due to gravity. The angular velocity vector ω resolved in thebody fixed frame is related to the generalized velocities η by means of the kinematic relationship
η = W−1v ω, (1.3)
where
Wv =
− sin θ 0 1cos θ sinψ cosψ 0cos θ cosψ − sinψ 0
.Define
J = J(η) = W Tv IWv, (1.4)
so thatTrot = 1
2 ηTJ ˙eta.
Thus, the matrix J = J(η) acts as the inertia matrix for the full rotational kinetic energy of thequadcopter expressed in terms of the generalized coordinates η.The model of the full rotorcraft dynamics is obtained from Euler- Lagrange equations withexternal generalized forces
d
dt
∂L∂q− ∂L∂q
=[Fξτ
], (1.5)
where Fξ = RF ∈ R3 is the translational force applied to the quadcopter due to main thrust, τ ∈R3 represents the yaw, pitch and roll moments, and R denotes the rotational matrix R(ψ, θ, φ) ∈SO(3) representing the orientation of the quadcopter relative to a fixed inertial frame.Thus
F =
00u
where u is the main thrust directed out the top of the aircraft expressed as
u =4∑i=1
fi
1
and, for i = 1, ..., 4, fi is the force produced by motor Mi. Typically, fi = kω2i , where k is a
constant and ωi is the angular speed of the ith motor. We assume that the center of gravity islocated at the intersecting motors M2 and M4. The generalized torques are thus
τ =
τψτθτφ
,
∑4i=1 τMi
(f2 − f4)l(f3 − f1)l
where l is the distance between the motors and the center of gravity, and τMi
is the momentproduced by motor Mi, i = 1, ..., 4, around the center of gravity of the quadcopter. Thus, weobtain
mξ +
00mg
= Fξ = RF (1.6)
Jη + C (η, η)η = τ (1.7)where
C (η, η) = J− 12∂
∂η(ηTJ)
is the Coriolis term, which contains the gyroscopic and centrifugal terms associated with the ηdependence of J.Finally, we obtain from 1.6 and 1.7
mx = −u sin θ, (1.8)my = u cos θ sinφ, (1.9)
mz = u cos θ cosφ−mg, (1.10)ψ = τψ (1.11)θ = τθ (1.12)φ = τφ (1.13)
where x and y are coordinates in the horizontal plane, z is the vertical position, and τψ,τθ, andτφ are the yawing moment, pitching moment, and rolling moment, respectively. These momentsare related to the generalized torques τψ,τθ,τφ by
τ =
τψτθτφ
= J−1(τ − C (η, η)η)
Once the system is modelled, for convenience it is better to express the above equations in thestate-space form:
x(t) = f(x(t),u(t))y(t) = g(x(t),u(t))
(1.14)
where the state vector is defined asx = [ξ v η ω]T
where:
ξ =
xyz
v =
xyz
η =
θφψ
ω =
θφψ
2
2. Linear model of quadcopterWe consider a linear state-space of the form:
x(t) = Ax(t) + Bu(t)y(t) = Cx(t) + Du
(2.1)
where x(t) is the vector state, y(t), the output vector, u(t) the input vector, B the input matrixand C the output matrix. D is considered null for simplicity.
To obtain, the linear state-space model, we need to linearise in an equilibrium point.Considering a model with small displacements around the nominal point:
x(t) = x0 + δx(t)u(t) = u0 + δu(t)
(2.2)
if (δx)i � (δx) and (δu)i � (δu) for i > 1 and considering smooth functions f(t) and g(t), todo the truncated Taylor series, we get:
f(x(t),u(t)) = f [x0,u0] + fxδx+ fuδu
g(x(t),u(t)) = g[x0,u0] + gxδx+ guδu(2.3)
where fx, fu, gx, gu are the Jacobian matrices.Hence the solution to the above expressions is obtained by the linearised equations:
δx = Aδx + Bδu
δy = Cδx(2.4)
where:A = ∂f
∂x
∣∣∣∣∣(X,U)
B = ∂f
∂u
∣∣∣∣∣(X,U)
C = ∂g
∂x
∣∣∣∣∣X
Considering the equations of the non linear model (1.11)-(1.13) and considering an operatingpoint (φ0, θ0, ψ0, φ0, θ0, ψ0, u0, τ 0
φ , τ0θ , τ
0ψ) = (0, 0, ψ0, 0, 0, 0, mg, 0, 0, 0)
δφ = φ ' δφ
δθ = θ ' δθ
δψ = ψ ' δψ
δφ = φ ' δτφ
δθ = θ ' δτθ
δψ = ψ ' δτψ
. (2.5)
For purposes of the Model Predictive control, it is convenient to express the above system in thestate equations, then
xR = ARxR + BRuR
y = CRxR
(2.6)
3
where:
AR =
0 0 0 1 0 00 0 0 0 1 00 0 0 0 0 10 0 0 0 0 00 0 0 0 0 00 0 0 0 0 0
BR =
0 0 00 0 00 0 01 0 00 1 00 0 1
CR =
1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 0
where AR is the rotational matrix with dimension R6×6, BR is the input matrix of the attitudesystem, with dimension R6×3 and CR is the output matrix of the attitude system, with dimen-sion R3×6. On the other hand xR is the state vector composed by: xR = (φ, θ, ψ, φ, θ, ψ), uR
is the input vector composed by: uR = (τφ, τθ, τψ), and yR is the output vector composed by:yR = (φ, θ, ψ).
Considering the equations (1.8)-(1.10) and an operating point:(x0, y0, z0, x0, y0, z0, u0, τ 0
φ , τ0θ , τ
0ψ) = (0, 0, 0, 0, 0, 0, mg, 0, 0, 0):
δx = x ' δx
δy = y ' δy
δz = z ' δz
δx = x ' (δθ)gδy = y ' −(δφ)g
δz = z ' (δf) 1m
. (2.7)
Again, to purposes of model predictive control we express the above equations in the form:
xT = AT xT + BT uT
y = CT xT
(2.8)
where:
AT =
0 0 0 1 0 00 0 0 0 1 00 0 0 0 0 10 0 0 0 0 00 0 0 0 0 00 0 0 0 0 0
BT =
0 0 00 0 00 0 0g 0 00 −g 00 0 1
m
CT =
1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 0
where AT is the state matrix of the position system, with dimension R6×6, BT is the input matrixwith dimension R6×3 and CT the output matrix with dimension R3×6. On the other hand xT isthe state vector of the system position composed by: xT = (x, y, z, x, y, z), uT is the inputvector composed by: uT = (φ, θ, f), and yT the output vector composed by: yR = (x, y, z).
4
3. Control AlgorithmWe can consider the equations (2.5) and (2.7) as chain of integrators of the form:
x1 = α1x2...
xn−1 = αn−1xn
xn = αnu
(3.1)
with αi 6= 0, ∀i ∈ [1, n]. Introducing the following change of coordinates:
y1 = x1
y2 = α1x2...
yn = αn · · ·αn−1xn
u′ = αn · · ·αnu
(3.2)
then, the system (3.2) becomes in
y1 = y2...
yn = u′
(3.3)
Define a function of saturation σbi of the form ( Figure 3.1):
σi(s) =
−bi para s < −bis para −bi ≤ s ≤ bi
bi para s > bi
(3.4)
where b > 0 .
Figura 3.1: Bounded function
The control lawu′ = −σbi(kiyi) (3.5)
5
globally asymptotically stabilizes the system (3.3) for all ki > 0 constant and bounds every state,yi, using the nonlinearity σbi.Rewriting (3.4) and (3.5) for the quadrotor system we obtain:
τθ = −σθ1(kdθθ)− σθ2(kpθ(θ − θd))︸ ︷︷ ︸uθ
+ux︷ ︸︸ ︷
σx1(kdxx) + σx2(kpx(x− xd)) (3.6)
τφ = −σφ1(kdφφ)− σφ2(kpφ(φ− φd))︸ ︷︷ ︸uφ
−uy︷ ︸︸ ︷
σy1(kdyy)− σy2(kpy(y − yd)) (3.7)
τψ = −σψ1(kdψψ)− σψ2(kpψ(ψ − ψd))︸ ︷︷ ︸uψ
(3.8)
u = −σz1(kdz z)− σz2(kpz(z − zd))︸ ︷︷ ︸uz
(3.9)
where τi are the torques applied to the vehicle.With this control law we can solve the attitude problem (uψ, uθ, uφ) and the position problem(ux, uy, uz).
4. A Basic Formulation of Predictive Control
4.1. State-Space Models4.1.1. Form of the model
In most of this paper we will assume a linearised, discrete-time, state-space model of theplant, in the form
x(k + 1) = Ax(k) +Bu(k) (4.1)y(k) = Cyx(k) (4.2)z(k) = Czx(k).k) (4.3)
where x is an n−dimensional state vector, u is an l−dimensional input vector, y is an my−dimensional vector of measured outputs, and z is an mz− dimensional vector of outputs whichare to be controlled, either to particular set-points, or to satisfy some constraints, or both. Thevariables in y and z will usually overlap to a large extent, and frequently they will be the same,that is, all the controlled outputs will be frequently measured. We will often assume that y ≡ z,and we will the use C to denote both Cy and Cz, and m to denote both my and mz. The indexk counts ‘time steps’.
We are going to assume that the sequence of actions at time step k is the following:
Obtain measurements y(k).
Compute the required plant input u(k).
Apply u(k) to the plant.
6
4.2. A Basic FormulationFor the basic formulation of predictive control we shall assume that the plant model is linear,
that the cost function is quadratic, and that constraints are in the form of linear inequalities.We shall also assume that everything is time-invariant. Furthermore, we shall assume that thecost function does not penalise particular values of the input vector u(k), but only changes ofthe input vector, ∆u(k). This formulation coincides with what is used in the majority of thepredictive control literature.
Cost function
The cost function V penalises deviations of the predicted controlled outputs y(k+i|k)) from areference trajectory r(k+ i|k). The notation indicates that this reference trajectory may dependon measurements made at time k. We define the cost function to be
V (k) =Hp∑
i=Hw‖y(k + i|k)− r(k + i|k)‖2
Q(i) +Hu−1∑i=0‖∆u(k + i|k)‖2
R(i). (4.4)
The prediction horizon has length Hp, but we do not necessarily start penalising deviations ofy from r immediately (if Hw > 1), because there may be some delay between applying an inputand seeing any effect. Hu is the control horizon. We will always assume that Hu ≤ Hp, and that∆u(k + i|k) = 0 for i ≥ Hu.
The form of the cost function (6.1) implies that the error vector y(k + i|k) − r(k + i|k) ispenalised at every point in the prediction horizon, in the range Hw ≤ i ≤ Hp. This is indeed themost common situation in predictive control. But it is possible to penalize the error at only afew coincidence points, by setting Q(i) = 0 for most values of i.We also need R(i) ≥ 0 to ensure that V (k) ≥ 0. We do not insist on the stronger condition thatR(i) > 0, because there are cases in which the changes in the control signal are not penalised.The weighting matrices R(i) are sometimes called move suppression factors, since increasingthem penalises changes in the input vector more heavily.
The cost function (6.1) only penalises changes in the input vector, not its value.The prediction and control horizonsHp andHu, the ‘window’parameterHw, the weightsQ(i) andR(i) an the reference trajectory r(k+ i), all affect the behaviour of the closed-loop combinationof plant and predictive controller.
Constraints
In the following equation we use vec(0) to denote a column vector, of which every element is0. We also use vec(a, b, c) to denote the vector [a, b, c]T , etc. We assume that constraints ofthe following form hold over the control and prediction horizons:
E vec(∆u(k|k), ...,∆u(k +Hu− 1|k), 1) ≤ vec(0) (4.5)
F vec[u(k|k), ..., u(k +Hu− 1|k), 1] ≤ vec(0) (4.6)
G vec[y(k +Hw|k), ..., y(k +Hp|k)] ≤ vec(0). (4.7)
7
Here E, F are matrices of suitable dimensions. We can use constraints of this form to representpossible actuator slew rates (4.5), actuator ranges (4.6), and constraints on the controlled varia-bles (4.7).
The important thing about the constraints is that they are all in the form of linear inequa-lities. When we want to solve the predictive control optimization problem, all these inequalitieswill need to be translated into inequalities concerning ∆u(k + i|k). Since we assume a linearmodel, we will see that these inequalities remain linear, even after this translation. This is goingto be crucial in order to be able to solve optimization problem reliably and efficiently.
5. Prediction
5.1. No disturbances, Full state measurementWe assume that the whole state vector is measured, so that x(k) = y(k)(C = I). Also we
asume that we know nothing about any disturbances or measurement noise. Then all we can dois to predict by iterating the model (4.2). So we get
x(k + 1|k) = Ax(k) +Bu(k|k) (5.1)x(k + 2|k) = Ax(k + 1|k) +Bu(k + 1|k) (5.2)
= A2x(k) + ABu(k|k) +Bu(k + 1|k) (5.3)
...
x(k +Hp|k) = Ax(k +Hp − 1|k) +Bu(k +Hp − 1|k) (5.4)= AHpx(k) + AHp−1Bu(k|k) + · · ·+Bu(k +Hp − 1|k) (5.5)
Recall that ∆u(k + i|k) = u(k + i|k) − u(k + i − 1|k), and that at time k we already knowu(k − 1). So we have
u(k|k) = ∆u(k|k) + u(k − 1)u(k − 1|k) = ∆u(k + 1|k) + ∆u(k|k) + u(k − 1)
...u(k +Hu − 1|k) = ∆u(k +Hu − 1|k) + · · ·+ ∆u(k|k) + u(k − 1)
and hence we get
x(k + 1|k) = Ax(k) +B[∆u(k|k) + u(k − 1)]x(k + 2|k) = A2x(k) + AB[∆u(k|k) + u(k − 1)] +B [∆u(k|k) + u(k − 1)]︸ ︷︷ ︸
u(k+1|k)
= A2x(k) + (A+ I)B∆u(k|k) +B∆u(k + 1|k) + (A+ I)Bu(k − 1)...
x(k +Hu|k) = AHux(k) + (AHu−1 + · · ·+ A+ I)B∆u(k|k)+· · ·+B∆u(k +Hu − 1|k) + (AHu−1 + · · ·+ A+ I)Bu(k − 1)
8
then
x(k +Hu + 1|k) = AHu+1x(k) + (AHu + · · ·+ A+ I)B∆u(k|k)+· · ·+ (A+ I)B∆u(k +Hu − 1|k) + (AHu + · · ·+ A+ I)Bu(k − 1)...
x(k +Hp|k) = AHpx(k) + (AHp−1 + · · ·+ A+ I)B∆u(k|k)+· · ·+ (AHp−Hu + · · ·+ A+ I)B∆u(k +Hu − 1|k) + (AHp−1 + · · ·+ A+ I)B∆u(k − 1)
Finally we can write this in matrix-vector form:
x(k + 1|k)...
x(k +Hu|k)x(k +H + 1|k)
...x(k +Hp|k)
=
A...
AHuAHu+ 1
...AHp
x(k) +
B...∑Hu−1
i=0 AiB∑Hui=0 A
iB...∑Hp−1
i=0 AiB
u(k − 1)
︸ ︷︷ ︸past
+
B · · · 0AB +B · · · 0
... . . . ...∑Hu−1i=0 AiB · · · B∑Hui=0 A
iB · · · AB +B... ... ...∑Hp−1
i=0 AiB · · · ∑Hp−Hui=0 AiB
∆u(k|k)
...∆u(k +Hu− 1|k)
︸ ︷︷ ︸future
.
(5.6)
The predictions of y are now obtained simply as
y(k + 1|k) = Cyx(k + 1|k) (5.7)y(k + 2|k) = Cyx(k + 2|k) (5.8)
(5.9)
...
y(k +Hp|k) = Cyx(k +Hp|k) (5.10)
or y(k + 1|k)y(k + 2|k)
...y(k +Hp|k)
=
Cy 0 · · · 00 Cy · · · 00 0 · · · Cy
x(k + 1|k)
...x(k +Hp|k)
. (5.11)
A warning is in order about the prediction equation (5.6). It involves computing Ai, possibly forquite large values of i. This can lead to numerical problems. If the plant is unstable, then someelements in Ai may become extremely large relative to others, and relative to elements in lowerpowers of A. Similar problems can occur if the plant is stable; in this case some elements of Aimay become extremely small relative to others. Again wrong answers may result.
9
6. Solving Unconstrained Predictive Control ProblemsRecall that the cost function which we must minimise is
V (k) =Hp∑
i=Hw‖y(k + i|k)− r(k + i|k)‖2
Q(i) +Hu−1∑i=0‖∆u(k + i|k)‖2
R(i). (6.1)
We can rewrite this asV (k) = ‖Y (k)−R(k)‖2
Q(i) + ‖∆U(k‖2R(i) (6.2)
where
Y (k) =
y(k +Hw|k)
...y(k +Hp|k)
, R(k) =
r(k +Hw|k)
...r(k +Hp|k)
, ∆U(k) =
∆u(k|k)
...∆u(k +Hu− 1|k)
.and the weighting matrices Q and R are given by
Q =
Q(Hw) 0 · · · 0
0 Q(Hw + 1) · · · 0... ... . . . ...0 0 · · · Q(Hp)
, R =
R(Hw) 0 · · · 0
0 R(Hw + 1) · · · 0... ... . . . ...0 0 · · · R(Hu− 1)
.
Also recall from (5.6) and (5.11), Y has the form
Y (k) = Ψx(k) + Υu(k − 1) + Θ∆U(k) (6.3)
for suitable matrices Ψ, Υ and Θ. Define
ε = τ(k)−Ψx(k)−Υu(k − 1) (6.4)
This can be thought of as a tracking error, in the sens that it is the difference between the futuretarget trajectory and the free response of the system. If ε really were 0, then it would indeed becorrect to set ∆U(k) = 0. Now we can write
V (k) = ‖Θ∆U(k)− ε(k)‖2Q + ‖∆U(k)‖2
R (6.5)= [∆U(k)TQε(k)]Q[Θ∆U(k)− ε(k)] + ∆U(k)TR∆U(k) (6.6)= ε(k)TQε(k)− 2∆U(k)TΘTQε(k) + ∆U(k)T [ΘTQΘ +R]∆U(k). (6.7)
But this has the form
V (k) = cte−∆U(k)TG+ ∆U(k)TH∆U(k). (6.8)
whereG = 2ΘTQε(k), H = ΘTQΘ +R.
and we can see that, neither G nor H depends on ∆U(k). To find the optimal ∆U(k) we cannow find the gradient of V (k) an set it to zero. from (6.8) we find
∇∆U(k)V = −G+ 2H∆U(k). (6.9)
so the optimal set of future input moves is
∆U(k)opt = 12H
−1G. (6.10)
10
In accordance with the receding horizon strategy, we use only the part of this solution corres-ponding to the first step. We can represent this as
∆u(k)opt = [Il, 0l, · · · , 0l]︸ ︷︷ ︸(Hu−1) times
∆U(k)opt (6.11)
where Il is the l × l identity matrix, and 0l is the l × l zero matrix.
6.1. Formulation as a Least-Squares ProblemThe optimal solution, as expressed in (6.11), should never be obtained by computing the
inverse of H. The matrix Θ is often ill-conditioned, which can result in H being ill-conditioned.The best way of computing the solution is by solving it as a least-square problem. It is also away which gives some additional insight.
Since Q ≥ 0 and R ≥ 0, we can find matrices SQ and SR which are their square-roots:
STQSQ = Q, STRSR = R
Now consider the vector [SQΘ∆U(k)− ε(k)
SRU(k)
](6.12)
We shall show that the squared length of this vector, or equivalently, the sum of squares of itselements, is the same as the cost function V (k), so that ∆U(k)opt is the value of ∆U(k) whichminimises this length.
‖[SQ(Θ∆U(k)− ε(k))
SRU(k)
]‖2 = ‖
[SQ(Z(k)− τ(k))
SRU(k)
]‖2 (6.13)
= [Z(k)− τ(k)T ]STQSQ[Z(k)− τ(k)] + ∆U(k)TSTRSR∆U(k) (6.14)= ‖Z(k)− τ(k)‖2
Q + ‖∆U(k)‖2R (6.15)
= V (k) (6.16)
So ∆U(k)opt is the least-squares solution of the equation[SQΘ∆U(k)− ε(k)
SRU(k)
]= 0 (6.17)
or, equivalently, of: [SQSR
]∆U(k) =
[SQε(k)
0
](6.18)
We can see equation (6.18) has the form AΘ = b and can be solved in a least-square sense usingthe QR algorithm. Although formally this solution is the same as θopt = (ATA)−1AT b, whichgives the equation (6.10).The predictive controller, for unconstrained problem, can be drawn as in the Figure 6.1
11
Figura 6.1: Structure of controller with no constraints
6.2. Obstacle avoidanceIn order to avoid obstacle, two edges are considered as depicted in Figure 6.2. The first edge
dD is the dangerous zone, when the vehicles approach the obstacle and enter in this zone, then acost function is minimizedto obstacle avoidance as well as reducing the distance between themto second edge i.e. dM . The purposed cost function is:
L0(k + 1) =
0 lsi ≥ dD
Np∑i=1−a(lsi (k + s|k)− dM) lsi < dD
(6.19)
where:
lsi =√
(xxi − x0)2 + (xyi − y0)2 −R0,
lsi (k + s|k) =√
(xxi (k + s|k)− x0)2 + (yyi (k + s|k)− y0)2 −R0
(6.20)
where a is a positive constant and (x0, y0), R0 are the Cartesian coordinates and the radius ofobstacle, respectively. xxi and yyi are the actual positions of the quadcopter in each instant oftime and (xxi (k+s|k) and (yyi (k+s|k) are the trajectories generated by model predictive controlin each position. The cost function to minimized to generate trajectories and avoid obstacles is
Figura 6.2: Evasión de obstáculo
defined by:J(k) = V (k) + L(k) (6.21)
12
where V (k) is the cost function shown in (6.1). The control structure to generated trajectoriesand avoid obstacles is shown in Fig 6.3.
Figura 6.3: Control structure of trajectory generation and obstacle avoidance
7. Formation Keeping UAVs
7.1. Formation Control modelFormation keeping is the most important to UAV formation flight. There are various kinds
of methods to solve the formation flight control problem. This paper use distributed structurecontrol, which put one of the UAV#1 as leader, its main task is to track the reference path, theothers UAVs (UAV#2, UAV#3) are the followers. The tracking strategy is that every UAV putthe nearest UAV as tracking target and follow it. In order to keep formation , follower shouldcalculate a virtual point as the certain position which it must hold in the flight formation.Put three UAVs compose an equilateral triangle formation as an example, the formation keepingand control form is shown in Fig 7.1. where ld is the expected distance between UAVs, (xi, yi)
Figura 7.1: UAVs formation keeping geometrical relationship
is the position coordinates of UAV #i, α is the yaw angle of UAV#i, 2 is the virtual point ofUAV#2, hence 3 is the virtual point of UAV#3.Let’s suppose, when UAV#1 track a reference path, the position coordinate at time t is (xd(t), yd(t)).The reference path is generated in certain method according to task and circumstances.According to the geometrical relationship shown in Fig 7.1, the virtual point’s position coordi-
13
nates of UAV #2 is given by
x′2 = x1 −√
32 ld cosα1 −
12 ld sinα1
y′2 = y1 −√
32 ld sinα1 + 1
2 ld cosα1
(7.1)
Similar for virtual point’s position coordinates of UAV#3
x′3 = x1 −√
32 ld cosα1 + 1
2 ld sinα1
y′3 = y1 −√
32 ld sinα1 −
12 ld cosα1
(7.2)
The control structure for flight formation of UAVs is shown in Fig 7.2
Figura 7.2: Flight formation control structure
8. Experimental results
8.1. Position and attitude controlConsidering the control law in (3.9) we developed experimental tests on the quadcopter
Ar.drone Parrot. We considered the vehicle is stabilized on the air. The references path are: TheUAV starts in (0, 0), then (−2,−2), immediately (−2, 2),(2, 2),(2,−2) by last (−2,−2) to do asquare. Fig. 8.1 shows the reference trajectory and the trajectory of the UAV.
14
Figura 8.1: Square trajectory
On the other hand in the Figure 8.2 we can see the position x of the UAV and the reference.Figure 8.3 shows the position x of the UAV and the reference.
Figura 8.2: Position x of the vehicle
Figura 8.3: Position y fo the vehicle
We can see in 8.2 y 8.3 the convergence is slow. This is because of the saturation in the controllaw and therefore the angles of the vehicle are around ±10◦ (Fig 8.4).
15
Figura 8.4: Angles of the vehicle
8.2. Trajectory generation based on MPCUsing the equations of model predictive control we developed the same test, a square but
in this case the MPC generates an optimal trajectory between the references. The referencesare: The UAV starts in (0, 0), followed to (−2,−2), immediately (−2, 2),(2, 2),(2,−2) and then(−2,−2).
Figura 8.5: MPC trajectory
The Figure 8.5 shows the reference trajectory and the trajectory of the UAV where the referencetrajectory is generated by the MPC. On the other hand the Figure 8.6 and 8.7 shows the positions(x, y) of the vehicle. Finally in the Figure 8.8 we can see the angles of the vehicle along of thetrajectory generated by the MPC.
16
Figura 8.6: Position x of the vehicle
Figura 8.7: Position y of the vehicle
Figura 8.8: Angles of the vehicle
17
8.3. MPC: Obstacle avoidanceWe considered a virtual obstacle which will be avoided by the UAV as it reach the final
reference. The UAV starts in (−3, 0) and the final reference is (−2, 0). The coordinates of theobstacle are (0, 0) with r = 0.15.
Figura 8.9: virtual obstacle avoidance
The Figure 8.9 shows that when the vehicle enters in the zone ID the UAV is penalized for avoidthe obstacle.On the other hand the Figures 8.10 and 8.11 shows the positions (x, y) of the vehicle along ofthe trajectory. Again this trajectory i generated by MPC.
Figura 8.10: Position x of the vehicle
In the Figure 8.12 we can appreciate the behaviour of the angles of the UAV, which are around±6◦.
8.4. Flight formationConsidering a triangular formation according to 7.1 and (7.1) and (7.2). The vehicle starts
in the position (2, 0) and follows the next positions (2,−2), (2, 2), (2, 0), (2, 0), (1, 0), and finally(0, 0).
18
Figura 8.11: Posición y del vehículo
Figura 8.12: Angles of the vehicle
Figura 8.13: Position x of the leader
19
The Figure 8.13 shows the reference and position of the leader in the x-axis. The leaderfollows the reference: x = 2, x = 3, x = 2, x = 1, y x = 0. On the other hand, in the Figure8.14 we can see the position y and the leader follows the reference: y = 0, y = −2, y = 2, yy = 0. In the Figure 8.15 we have the behaviour of the leader in the plane xyz.
Figura 8.14: Position y of the leader
We can see in the Figure 8.16 the position of the follower 0 around the x−axis. The follower
Figura 8.15: Position 3D of the leader
generates its own virtual points, which are: x = 0.87, x = 1.87, x = 0.87, x = −0.12 andx = −1.12, considering ld = 1.3 m. On the other hand, the Figure 8.17 shows the position of thefollower 0 around the y−axis. It generates the virtual points: y = 0.65, y = −1.35, y = 2.65,and y = 0.65. For appreciate the behaviour of the follower 0, Figure 8.18 shows the trajectoryin 3D.
20
Figura 8.16: Position x of the follower 0
Figura 8.17: Position y of the follower 0
Figura 8.18: Position 3D of the follower 0
21
In the same way the Figure 8.19 shows the position of the follower 1 with virtual points:x = 0.87, x = 1.87, x = 0.87, x = −0.12 and x = −1.12.
Figura 8.19: Position x of the follower 1
On the other hand in the Figure 8.20 we can observe the virtual points of the follower 1 aroundthe y−axis, with virtual points: y = −0.65, y = −2.65, y = 1.35 and 0.65.
Figura 8.20: Position y of the follower 1
Figure 8.21 shows the trajectory traveled in the 3D.
To appreciate the behaviour of the quadcopter’s angles, we can see the Figure 8.22.
Finally to observe the trajectory travelled by the three UAVs, we can see the Figure 8.23which represent a triangular formation along the reference trajectory.
22
Figura 8.21: Position 3D of the follower 1
Figura 8.22: Ángulos de los 3 UAVs
Figura 8.23: Flight formation
23