OPTIMAL TRAJECTORY PLANNING

AND LQR CONTROL

FOR A QUADROTOR UAV

Ian Cowling
James Whidborne
Alastair Cooke
Fixed Camera Onboard Camera

Draganflyer
Contents
Quadrotor dynamics including differentially flat dynamics

Trajectory planning using a suitable parameterization

Trajectory following using LQR control

Results for a number of missions

Quadrotor dynamics
4 Control inputs
U1 F1 F2 F3 F4
U 2 l ( F3 F4 )
U 3 l ( F1 F2 )
U 4 1 2 3 4
where Fn is thrust from nth rotor,
l is arm length
n is torque from nth rotor
 Quadrotor dynamics
4 Outputs

y [ x, y, z, ]

12 States

x [ x, y, z, x, y , z, , , , ,, ]

where [ x, y, z ] are translational

positions,
is pitch angle,
is roll angle,
is yaw angle.
Rotational matrix
Typically the equations of motion of air vehicles are determined using
the standard aeronautical rotational matrix Rxyz
For the quadrotor the equations of motion can be simplified using an
alternative rotational matrix Rzyx

Translational equations of motion

using Rzyx
x U1 (sin cos )
y U1 ( sin )
z g U1 (cos cos )
Missions
Vertical flight
Climb 5m in 7 seconds with crosswind

Obstacle avoidance
Fly 6m north in 15 seconds avoiding obstacle with tailwind

Mineshaft mission
Fly 10m north and drop down mineshaft with tailwind
Differential Flatness
Differential flatness is a dynamic property within some dynamic
systems which allows for the expression of state and input as a
function of output and its derivatives.

The roll and pitch angles can be expressed as a function of the

output as shown
x
arctan
g z
y
arcsin

U1
Trajectory Generation
Differentially flat equations enable trajectory generation
to occur within output space as opposed to control space.

Optimization to determine the optimal trajectory to reach the destination

at the end of the time horizon with the lowest possible fuel consumption

Subject to dynamic, actuator, initial and terminal constraints

min for t 0, T
s.t. c y 0;
x0 h1 ( y (0)) 0
yT y (T ) 0
Parameterization of output space
Reduce problem to finite dimensional problem

Product of free variable and basis function

The polynomial is probably the most widely used basis function but there
are alternatives which can improve conditioning

M
f (t ) ak k (t )
k 0

where ak is a free variable, M is the order of the basis function and

is a basis function. The search space hence becomes R4(M+1)
Possible parameterizations
Polynomials k t k

Taylor series polynomials (improves conditioning) k (1 / k!)t k

Chebyshev polynomials k ( x) cos(ny ) where x cos( y )
Laguerre polynomials 0 (t ) 1
1 (t ) 1 t
(k 1)K 1 (t ) (2 K 1 t )K (t ) kK 1 (t )
Comparison of basis functions
Tested each basis function for a number of missions. The number of iterations
required to determine find the optimal trajectory is recorded for comparison

All three techniques require fewer iterations than the polynomials

Chebyshev polynomials are

cylindrical in nature therefore the
initial and terminal boundary Iterations The Well Obstacle Vertical
conditions can be determined
analytically Polynomial 30 85 17

Laguerre 21 33 18
polynomial
Chebyshev 15 33 17
polynomial
Taylor 30 30 20
LQR control
Recalling the standard LQR problem for a linear state-space model
of the plants dynamics
x (t ) Ax (t ) Bu (t )
A control input is determined
u (t ) uref K c x(t )
So that the closed loop system
x [ A BK c ]x(t )
Is stable and the performance measure is minimized

J ( x(t )Qx(t ) u (t ) Ru (t )) dt
0
Trajectory following
Following time dependent reference trajectory xref

The model is linearized to determine the gains for LQR control

The reference control signal from the trajectory optimization is fed forward

u (t ) uref (t ) k ( x(t ) xref (t ))

where uref and xref are the reference control signal and reference state from
the trajectory optimization. k is the LQR gain matrix obtained. x is the state
matrix.
Simulation model
Simulation model is used to validate control algorithms

Model parameters have been determined experimentally

by some of our colleagues in the Dynamic, Simulation
and Control group

Model includes experimental values Clock

TIME

To Workspace
10
Out10

of mass, inertias and voltage to thrust

0.444
11
Mass Out11

0.008336624
12
IXX
Out12

relationship
1
0.007148562
Out1
IYY

0.012464701

IZZ north 2
phi theta psi east Out2
0
altitude

IXY northdot
mass properties
3
eastdot
XY Z L MN uv w
0 altdot Out3
phi theta psi

IYZ nav igation

grav ity

0
mass properties uv w
IZX 16
(X Y Z L M N) - grav ity pqr Out16
0

XCG (X Y Z L M N) - powerplant pdot, qdot, rdot Demux 17

Out17
equations of motion
0

YCG 18
Out18
0

ZCG
4
Out5

phi theta psi Demux 5

Out6
pqr
1
In1 Phidot, Thetadot Psidot 6
Out7
7
2 angular motion
transf ormation Out4
In2
motor v oltages XY Z L MN
13
3 Out13 Demux 8
In3 Out8
powerplant
Demux 14
4
Out14 9
In4
Out9
15
Out15
Results
Obstacle avoidance fly from [0,0,0] to destination at
[6,0,0] in 15 seconds with a tailwind and avoid the obstacle.
Results
Mineshaft mission fly from [0,0,0] to the top of and down
the mineshaft in 25 seconds with a tailwind
Conclusions
Differentially flat dynamics enable efficient trajectory
optimization within the output space

Laguerre polynomials to improve conditioning of output

space parameterization

LQR control to follow reference trajectory

Future work
Presented MBPC control scheme at 21st UAV Systems conference

Combined control (LQR/MBPC). If the reference trajectory becomes infeasible

or suboptimal, reoptimization is required to determine a new trajectory.

Multi-vehicle flight. Multi vehicle rendezvous may be required for refuelling

or group manoeuvring. The circumcentre law guarantees rendezvous of multiple
vehicles with limited sensor range and no communication between vehicles.

Automatic Differentiation. A significant reduction in computation time

for trajectory optimization can be achieved by supplying the gradients
of the constraints into the optimization routine. AD is an alternative approach
to the calculation of these gradients where the analytical solution by hand is
time consuming or impossible.

Real platform testing on the Draganflyer X-Pro.