See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/238677911

OPTIMAL TRAJECTORY PLANNING AND LQR CONTROL FOR A QUADROTOR

UAV

Conference Paper · September 2006

CITATIONS READS

61 1,081

3 authors:

Ian Cowling James Whidborne

17 PUBLICATIONS 376 CITATIONS

Cranfield University
265 PUBLICATIONS 3,362 CITATIONS
SEE PROFILE
SEE PROFILE

Alastair Cooke
Cranfield University
52 PUBLICATIONS 679 CITATIONS

SEE PROFILE

Some of the authors of this publication are also working on these related projects:

Structured uncertainty analysis of pole placement and H∞ controllers for directional drilling attitude tracking View project

An Exergy Analysis Approach for Flexible Aircraft View project

All content following this page was uploaded by James Whidborne on 02 September 2015.

The user has requested enhancement of the downloaded file.

 OPTIMAL TRAJECTORY PLANNING AND LQR
CONTROL FOR A QUADROTOR UAV

Ian D. Cowling James F. Whidborne Alastair K. Cooke

Department of Aerospace Sciences,

Cranfield University,
Bedfordshire, MK43 0AL, U.K

Abstract: As research into UAVs accelerates into the 21st century, alternatives to fixed
wing vehicles such as the quadrotor are causing interest. The quadrotor is a small agile
vehicle which could be suitable for search and rescue, surveillance and remote inspection.
For autonomous operation a control system that incorporates both trajectory planning
and trajectory following is required. Trajectory planning can be posed as a constrained
optimization problem typically within the control space and with some constraints being
placed in the output space. However, differential flatness enables the optimization to occur
within the output space and therefore simplifies the problem. A parameterization of the
output is required to reduce the problem to a finite dimensional problem, this can be done
using any number of techniques. Trajectory following can be achieved using linear multi-
variable control techniques such as LQR control.

Keywords: Quadrotor, Optimal trajectory planning, Laguerre polynomials, Differential

flatness, Trajectory following, LQR.

1. INTRODUCTION the output space, as opposed to the control space. This

technique has been considered for air vehicles (Martin
et al., 1994) and applied to a helicopter (Koo and Sas-
Unmanned air vehicles (UAVs) have attracted consid- try, 1999). Differential flatness has also been consid-
erable interest in recent times. Fixed wing vehicles ered with respect to the quadrotor to achieve a conver-
have had extensive applications for military and me- gent tracking controller (Driessen and Robin, 2004).
teorological purposes. The quadrotor is a small agile
vehicle controlled by the rotational speed of the four Trajectory following can be achieved using a LQR
rotors. It benefits from having very few constraints controller to follow a time dependent reference tra-
on motion and an ability to carry a high payload. jectory xtra j . In this paper, non-linear optimization
Furthermore, with the use of ducted fans instead of is used to determine the optimal trajectories for the
prop rotors it would be safe for internal flight. The low quadrotor to execute some simple missions. An LQR
cost and simplicity mean the quadrotor provides an controller is then used to follow the generated tra-
excellent testing ground for application of advanced jectory. By means of simulation, it is shown that the
control techniques on UAVs. quadrotor follows the optimal trajectory, despite wind
and other perturbations.
In order to achieve full autonomy a controller has
to incorporate trajectory planning and trajectory fol- This paper is divided into 3 sections, the first discusses
lowing which is often regarded as a separate issue closed loop control and application of LQR to follow
(Richards and How, 2002). The path planning problem a reference trajectory. The second section derives the
can be simplified by exploiting the differential flatness differential flatness and formulates the trajectory plan-
of the vehicle dynamics. Differential flatness (Fleiss ner. Finally application of these techniques requires
et al., 1992) enables the optimization to occur within a output space parameterization which is considered
within the final section. The controller is then vali- where υi and τi are respectively the normalized torque
dated using a full simulation model of the quadrotor. and normalized thrust from the ith rotor, and l is the
distance of the rotor from the center of mass.

2. LQR TRAJECTORY FOLLOWING The state variable vector, x, is defined as

xT = x y z ẋ ẏ ż φ θ ψ φ̇ θ̇ ψ̇
 
(10)
In the standard LQR control problem, for a linear where x, y and z are the translational positions (see
state-space model of the plant dynamics Figure 1) and φ, θ, ψ are the roll, pitch and yaw
ẋ (t) = Ax (t) + Bu (t) , (1) respectively 1 . In order to determine the differential
flatness that will be discussed in section 4.2 it is
a control input
necessary to define four outputs. These are chosen
u (t) = uref − Kc x (t) (2) as the translational positions, x, y, z, and the yaw
angle, ψ. The yaw angle is chosen because it can be
is determined such that the closed loop system
dynamically decoupled from the other states (Castillo
ẋ = [A − BKc ]x (t) (3) et al., 2004). The output vector, y, is hence defined as
yT = x y z ψ .
 
is stable and a performance measure where (11)
Z∞
J= (x (t) Qx (t) + u (t) Ru (t) ) dt (4) The control vector, u, is defined as
0 uT = [u1 , u2 , u3 , u4 ] (12)
is minimized where Q and R are weighting matrices.
where u1 = ũ1 , u2 = φ̈, u3 = θ̈ and u4 = ψ̈.
Assuming a time dependent state reference trajectory
The control input u1 can be expressed as
xtraj (t), LQR control can be applied as a trajectory q
follower to minimize small errors between the full u1 = ẍ2 + ÿ2 + (g − z̈)2 (13)
measured state x and the reference state xtraj , such that
The actuator outputs can be expressed as functions of
the applied control is

the control and its integrals
u(t) = uref − Kc x(t) − xtraj (t) . (5)
φ̈
    
ũ2 cψ cθ sψ cθ −sψ
ũ3  =  −sψ cψ 0   θ̈ 
3. DYNAMIC MODEL ũ4 0 0 1 ψ̈
φ̇
  
−(sψ cθ + cψ sθ ) cψ cθ − sψ sθ −cψ
+ −cψ −sψ 0   θ̇  (14)
0 0 0 ψ̇
where cθ = cos(θ), sθ = sin(θ), etc.
From (13) we get the translational equations of motion
ẍ = u1 (sθ cφ ) (15)
ÿ = u1 (−sφ ) (16)
z̈ = g − u1(cθ cφ ) (17)

Taking the state equation (10) and the translational

equations of motion (15) – (17), a state space model
can be defined as
   
x ẋ
Fig. 1. Quadrotor schematic y 
   ẏ 

z  ż 
The quadrotor is actuated only by independently vary-
   
 ẋ   u1 (sθ cφ ) 
ing the speed of the four rotors. A pitch moment is
   
 ẏ   u1 (−sφ ) 
achieved by varying the ratio of the front and back
   
 ż  = g − u1(cθ cφ )
d    
(18)
rotor speeds, a roll by varying the left and right rotor dt  φ  
   φ̇ 
speeds (see Figure 1). A yaw moment is obtained from θ  θ̇


the torque resulting from the ratio of the clockwise ψ  ψ̇
   

(left and right) and the anti-clockwise (front and back)  φ̇  
   
u2 
speeds. The actuator outputs, ũi , are hence functions  θ̇  
   
u3 
of normalized individual thrusts and torques: ψ̇ u4
ũ1 = (τ1 + τ2 + τ3 + τ4 ) (6)
ũ2 = l(τ3 − τ4 ) (7) 1 Typically, the equations of motion of air vehicles are determined
ũ3 = l(τ1 − τ2 ) (8) using the standard aeronautical rotational matrix Rxyz (Cook, 1997).
However, for the quadrotor, they can be simplified by using the
ũ4 = (υ1 + υ2 − υ3 + υ4 ) (9) rotational matrix Ryxz (Castillo et al., 2004).
 obstacle avoidance constraints. For example, the prob-
60
Boundary of S lem could be posed as
Boundary of Sc

40 min Φ for t ∈ [0, T ]

û(t)∈u
20
s.t. cu (u) ≤ 0
(23)
Θ

0
s.t. x0 − g1 (u0 ) = 0
−20
s.t. yT − g2 (uT ) = 0
−40 s.t. y = g3 (u, x0 )
−60
where Φ is the cost function, cu (u) is a set of functions
−80 −60 −40 −20 0
Φ
20 40 60 80
that express inequality constraints on the state and
output, x0 is the initial state at t = 0, the state is a
function g1 of the input, yT represents the terminal
Fig. 2. Stability region for varying θ and φ output at t = T , the output is some a function g2 of
3.1 Stability analysis the input and some dynamic constraints are applied so
that the output y is a function g3 of the input u and the
The control gains Kc were designed with the plant initial state x0 .
linearized at hover with
Q = (1 × 10−5)I (19) 4.2 Differential flatness
−5 8 8 8
R = diag(1 × 10 , 1 × 10 , 1 × 10 , 1 × 10 ) (20)
Differential flatness is the expression of the state
The weighting matrices Q and R were chosen to en- and control vectors in terms of the output vector
sure that the actuator constraints would be maintained. (Fleiss et al., 1992). For a system to be differentially
Clearly, to follow a trajectory, the system does not flat and therefore possessing a flat output it requires
remain at hover. A simplified analysis is therefore per- (Chelouah, 1997) a set of variables such that;
formed to determine an envelope of operation where (1) the components of y are not differentially related
the vehicle will remain stable. The analysis is not over R;
rigorous and is hence only an indicator, however the (2) every system variable may be expressed as a
analysis is simple and provides a convex bound on function of the output y;
the state x. Stability for a calculated trajectory can be (3) conversely, every component of y may be ex-
subsequently checked by simulation. From (18), the pressed as a function of the system variables and
linearized dynamics depend on three variables, θ, φ of a finite number of their time derivatives.
and u1 . We define the linearized stability set S to be
By manipulation of the equation of motion and recall-
S = θ, φ : α(A(θ, φ, u1 ) − B(θ, φ)Kc ) < 0,

ing (13)-(14), the state vector and input vector can be
0.5 < u1 < u1(max) (21) expressed as a function of the output vector.
 
ẍ
where α(·) is the spectral abscissa (most positive real θ = arctan (24)
part of the eigenvalues). The set is plotted in Figure 2. g − z̈
 
By inspection, we can fit a disk inside the set, hence it −ÿ
is clear that S c ⊂ S where φ = arcsin (25)
u1
... ...
S c = θ, φ : θ2 + φ2 ≤ r2 x (g − z̈) + ẍ z

(22) θ̇ =   (26)
with r = 48◦ . S c is also shown in Figure 2. An extra (g − z̈)2 + ẍ2
constraint can be inserted into the trajectory planner, ...
(u̇1 ÿ − u1 y )
which maintains the angles within this set and there- φ̇ = p (27)
u1 u1 2 − ÿ2
fore ensures linearized time-invariant stability.
Singularities in this model only appear when g = z̈, in
other words when the vehicle is in free fall. This can
be avoided by constraining the input such that u1 > 1
4. TRAJECTORY OPTIMIZATION and the pitch and roll such that θ < 90◦ and φ < 90◦ .
These angles are outside the set S c defined by (22).
4.1 Problem formulation

To determine the optimal reference trajectory, typi- 4.3 Problem formulation

cally a optimization within the control space is per-
formed subject to some constraints placed within the From the differentially flat equations the problem can
output space and the state space. These constraints be reposed to allow optimizations to occur within the
include physical constraints, actuator constraints and output space as opposed to the control space. This is
beneficial because constraints such as obstacle avoid- however, can be expressed as a product of a free
ance occur in the output space, hence the computation variable(λ) and a basis function Γk ,
time for constraint handling is reduced. The problem M
is posed as follows: f (t) = ∑ ak Γk (t) (30)
k=0
min Φ for t ∈ [0, T ]
y(t) where M is the order of the basis function. The search
s.t. cy (y) ≤ 0, space hence becomes R4(M+1) .
(28)
x0 − h1 (y(0)) = 0, The polynomial basis function can be expressed as,
yT − y(T ) = 0
Γk = t k . (31)
where the inequality constraints are now expressed as Chebyshev polynomials can be defined as,
a function of the output cy (y) and the state is now a Γk (x) = cos(ny) where x = cos(y). (32)
function of the output obtained from the differential
flatness h1 (y). This problem can, with a suitable pa- Laguerre polynomials can be derived from recurrence
rameterization, be entered into MATLAB and solved relationship,
using the optimization toolbox function fmincon. Γ0 (t) = 1
Γ1 (t) = 1 − t
(k + 1)ΓK+1 (t) = (2K + 1 − t)Γk(t) − kΓk−1 (t).
4.4 Cost function (33)
From the Taylor series the conditioning of the polyno-
The objective function, Φ, is a quantitative measure of mials (31) can be improved using a basis scaling,
the optimality of the trajectory, which, can be approx-
imated by a measure of the running costs. Assuming Γk = (1/k!)t k . (34)
running costs are proportional to average velocity then Polynomials, Laguerre polynomials, Chebyshev poly-
the objective function can be defined as: nomials and Taylor series expansion polynomials have
Z been tested for 3 missions defined in (6.1) and the
1 T 1
Φ= (P1 ẋ2 + P2ẏ2 + P3ż2 ) 2 dt (29) results can be seen in Table (1)
T 0
where P1 , P2 , P3 are weighting factors. Table 1. Number of iterations for various
parameterization techniques
Iteration The well Vertical Obstacle
Polynomial 30 85 17
4.5 Constraints Laguerre 21 33 18
Chebyshev 15 33 17
For efficient computation the number of constraints Taylor 30 30 20
have to be kept small. From the differential flatness
property, the state and input trajectories, x and u re- Chebyshev polynomials are cylindrical in nature and
spectively, can be determined from the output trajec- therefore initial and final boundary conditions can be
tory. Inequality constraints on the output trajectory determined analytically and applied to reduce com-
(for obstacle avoidance), and on the state and input putation time although this proves complex for vari-
trajectories (to avoid singularities and to provide con- able horizon times. Taylor series expansions and La-
straints on the control signals) are expressed through guerre polynomials both prove efficient parameteriza-
the function cy (y). Initial constraints are placed on the tion techniques for this problem. Laguerre polynomi-
state at t = 0. Terminal constraints are also placed on als seem slightly better and are therefore chosen.
the output trajectory to ensure the vehicle reaches its
destination at t = T , these are expressed by y(T ) = yT .
6. SIMULATION WITH FULL MODEL

To validate the control strategy a full simulation

5. PARAMETERIZATION model of the quadrotor has been developed. The full
SIMULINK model incorporates experimental data
The optimization of the 4 outputs (x, y, z, ψ) must be and theoretical analysis. This model is used to test
parameterized in order to reduce the dimensions of the control algorithm to fly 3 missions. The model is
the problem to a finite amount. There are numer- described more fully in Cowling et al. (2006).
ous alternatives for this task such as the polynomial
(Nieuwstadt and Murray, 1995), Laguerre polynomi-
als (Huzmezan et al., 2001) and Chebyshev polyno- 6.1 Missions
mials (Vlassenbroeck and Van Dooren, 1988). Poly-
nomials can also be conditioned using a basis scal- To test the control scheme, 3 missions have been
ing obtained from the Taylor series. Every technique, created to test the robustness and performance of the
 7. CONCLUSIONS
6

5
This paper presents an optimal trajectory planner with
a linear control scheme to follow a reference trajec-
4
tory. This scheme has been validated using a full dy-
Altitude, z (metres)

namic model of the quadrotor. Such a scheme can be

3

used to achieve real time autonomous behaviour of

2
unmanned air vehicles.

1 Flight Path
The disadvantages of linear trajectory following occur
Destination
when the trajectory becomes sub-optimal or infeasible
0
0 1 2 3 4 5 6 7 8
due to environmental changes. This will be considered
Time (seconds)

in future work by implementing a dual loop control

scheme. An outer loop will be a trajectory optimiza-
tion and the inner loop will be a stabilizing trajectory
follower. Future work will also extend the scheme to
Fig. 3. Vertical flight a larger quadrotor (Draganflyer X-Pro) and include
controller. All missions involve starting from hover dynamic modelling and flight testing.
at (0, 0, 0) and moving to a destination within the
time horizon with a sampling rate of 10Hz. To test the
robustness of the controller, disturbances have been REFERENCES
added, these include translational drift and random Castillo, P., A. Dzul and R. Lozano (2004). Real-time stabilization
gusting to simulate wind. To simulate inaccuracies and tracking of a four-rotor mini rotorcraft. IEEE Trans. Con-
trol Syst. Tech.
within the rotor, random noise is also added to the Chelouah, A. (1997). Extensions of differential flat fields and Li-
voltage input into the model. ouvillian systems. In: Proc. 36th IEEE Conf. Decision Contr..
San Diego, Caliornia.
(1) The first mission involves a vertical flight of 5m Cook, M.V. (1997). Flight Dynamics Principles. Butterworth
in 7 seconds with a cross wind of 0.01ms−1. Heinemann, Oxford, England.
(2) The second mission involves navigation around Cowling, I.D., J.F. Whidborne and A.K. Cooke (2006). MBPC for
Autonomous Operation of a Quadrotor Air Vehicle. In: Proc
an obstacle to reach a destination at (6,0,0) 21st International UAV Systems Conf. Bristol, UK.
within 15 seconds. The obstacle is modelled as Driessen, B.J. and A.L. Robin (2004). A globally convergent track-
a sphere centered at (3,0,0) with a radius of 1m. ing controller for the X4 flyer rotor craft for reference trajecto-
ries with positive thrust. In: Robotica Part 4. pp. 375–388.
A tail wind is modelled as a drift of 0.01ms−1.
Fleiss, M., J. Levine, Ph. Martin and P. Rouchon (1992). Sur les
(3) The third mission is a horizontal flight to (10,0,0) systemes non linerities differentiellement plats. C.R.Acad Sci.
followed by a descent down a mineshaft of radius Huzmezan, M., G.A. Dumont, W.A. Gough and S. Kovac (2001).
2m to the bottom(10,0,-5) within 25 seconds Multivariable Laguerre-based indirect adaptive predictive con-
trol a reliable practical solution for process control. In:
with a tail wind of 0.05ms−1. IASTED Modelling and Control. Innsbruck, Austria.
Koo, T.J. and S. Sastry (1999). Differential flatness based full au-
thority helicopter design. In: Proc. 38th IEEE Conf. Decision
Contr.. Phoenix, Arizona.
6.2 Results
Martin, P., S. Devasia and B. Paden (1994). A different look at
output tracking: control of a VTOL aircraft. In: Proc. 33rd
The first mission consists of a 5m vertical flight within IEEE Conf. Decision Contr.. Lake Buena Vista, Florida.
7 seconds. To increase the chance of a feasible flight Nieuwstadt, M.J. and R.M. Murray (1995). Apprximate trajectory
generation for differentially flat systems with zero dynamics.
path when optimising the trajectory, a tolerance of In: Proc. 34th IEEE Conf. Decision Contr.. New Orleans.
25cm is included in the terminal constraints. The Richards, A. and J.P. How (2002). Aircraft trajectory planning with
flight path and reference trajectory for this mission are collision avoidance using Mixed Integer Linear Programming.
In: Proc. 2002 Amer. Contr. Conf.. Anchorage, Alaska.
shown in Figure 3. The second mission involves the Vlassenbroeck, J. and R. Van Dooren (1988). A Chebyshev Tech-
navigation around an obstacle to reach the destination nique for Solving Nonlinear Optimal Control Problems. IEEE
within 15 seconds. As seen in Figure 4 the optimal Trans. Autom. Control 33, No. 4, 333–340.
path is very close to the obstacle, this is understand-
able as the optimal route is the shortest route. This
does show the need for a factor of safety allowance
into the path planning. The third mission involves a
horizontal flight to the top of a mineshaft and then a
vertical descent down to the bottom. The flight path
and reference trajectory can be seen in Figure. 5. The
wind acts on the vehicle when the vehicle is above
ground but not when the vehicle is in the mineshaft.
As seen, the vehicle drifts from the trajectory above
ground but still passes down the mineshaft without
hitting the walls.
 1

0.5
Altitude, z (metres)

−0.5

−1
1 6
5
4
0 3
2
−1 1
0
East, y (metres) North, x (metres)
Flight Path
Reference Trajectory
Destination
Obstacle

Fig. 4. Obstacle Avoidance

−1
Altitude, z (metres)

−2

−3

−4

−5 12

2 10
8
1
6
0
4 Flight Path
−1 Reference Trajectory
2
Mineshaft
−2
0 North, x (metres)
East, y (metres)

Fig. 5. Mineshaft descent

View publication stats