Path Planning and Control Utilizing Differential Flatness of RotorcraftJeff Ferrin, Robert Leishman, Randy Beard, and Timothy McLain

Abstract— We propose a new method to control a multi-rotor aerial vehicle. We show that the system dynamics aredifferential flat. We utilize the differential flatness of the systemto provide a feed-forward input. The system model derivedallows for arbitrary changes in yaw and is not limited to smallroll and pitch angles. We demonstrate in hardware the abilityto follow a highly maneuverable path while tracking a time-varying heading command.

I. INTRODUCTION

Simple rotorcraft are becoming increasingly utilized foraerial robotics research [18], [10], [19], [1], [9], [3]. Theseaircraft are well adapted for research as they are relativelyinexpensive, easily repairable, carry a substantial payload forthe size, and provide hover capability.

Path planning and control for rotorcraft has been studiedextensively as well. Hoffmann, et al. [5] implemented apath planning and control method that specifically avoidedutilizing feed-forward or dynamic feasibility portions. Theyreport indoor and outdoor hardware flight results. Indoorflight errors were at or below 0.1 m, in any one dimension,at a max speed of 0.5 m/s. Recently, researchers have beenable to complete complex maneuvers using these aircraft andmotion capture systems. Researchers with the GRASP Labhave developed control schemes for flips, landing on walls,and flying through narrow windows [14]. Researchers withthe ETH in Zurich have completed algorithms for playingand moving to music using quadrotors [20]. Both of theselatter approaches describe the need of feed-forward terms toperform such maneuvers, but do not further elaborate.

We use a motion capture system from Motion Analysis[16] and we are using hexacopter platforms by Mikrokopter[15]. We have implemented PID controllers that decouple theposition and velocity control[4], but there remains a need fora more robust controller. We will describe a method that willhandle aggressive maneuvers.

Fig. 1. Mikrokopter hexacopter platform.

Cowling, et al. [6] created a path planner and path followerbased on the principle of differential flatness. Essentially,a feed-foward input is obtained from the desired path andthe dynamics of the system. The authors implemented anoptimization scheme for online path planning and showsimulation results. However, the approach is limited to azero-degree yaw angle and to small roll and pitch angles.

We propose a new method of control utilizing the differen-tial flatness of a rotorcraft platform that allows for arbitrarychanges in yaw and is not limited to small roll and pitchangles. This scheme is shown to be adequate for aggressivemaneuvers. In Section II we outline the dynamic modelof the hexacopter system. We derive the differentially flatoutputs of the rotorcraft in section III. Section IV explainshow trajectories are generated and implemented. Section Vdiscusses the LQR controller that closes the loop. We showsthe results that we achieved in section VI, and finally, wrapup with conclusions in section VII.

II. DYNAMIC MODEL

We first set up the notation and coordinate systems thatwill be used in the derivation of the equations of motion forthe hexacopter. A vector v in frame a will be denoted asva and a rotation matrix that rotates a vector from framea to frame b is Rba. Therefore the vector v in frame b isvb = Rbav

a. The two coordinate frames that we will useare the inertial frame (frame i) and the body frame (frameb), shown in Figure 2. The inertial frame is a north-east-down reference frame where the unit vector ii is alignedwith the north direction, ji is aligned with the east directionand ki is aligned with the down direction. The body frameis a reference frame that is attached to the hexacopter. Thisframe is related to the inertial frame through the Euler anglesφ, θ and ψ, where φ is the roll angle, θ is the pitch angleand ψ is the vehicle heading. The body frame is found byfirst rotating through the yaw angle ψ then through the pitchangle θ and then through the roll angle φ. The rotation matrixfrom the body frame to the inertial frame as derived in [2]is

Rib(ψ, θ, φ) = R(ψ)R(θ)R(φ) (1)

where R(ψ), R(θ) and R(φ) are the individual rotationmatrices for the respective angles, given by

R(φ) =

1 0 00 cosφ sinφ0 − sinφ cosφ

, (2)

R(θ) =

cos θ 0 − sin θ0 1 0

sin θ 0 cos θ

, (3)

and

R(ψ) =

cosψ sinψ 0− sinψ cosψ 0

0 0 1

. (4)

Multiplying each matrix in Equation (1) gives

Rib(ψ, θ, φ)=

cθcψ sφsθcψ − cφsψ cφsθcψ + sφsψcθsψ sφsθsψ + cφcψ cφsθsψ − sφcψ−sθ sφcθ cφcθ

(5)

where cθ4= cos θ and sθ

4= sin θ.

Fig. 2. Hexacopter coordinate systems.

There are 12 state variables that will be used in derivingthe equations of motion. These states are

x =[pn pe pd pn pe pd φ θ ψ p q r

]T,

where pn, pe and pd are the north, east and down positions,respectively, in the inertial coordinate frame, pn, pe, pd arethe inertial velocities, φ, θ and ψ are the Euler angles andp, q and r are the angular rates along the ib, jb and kb axesrespectively. The position of the hexacopter (pn, pe, pd) isa simple kinematic relationship with the time derivatives ofposition, which gives

d

dt

pnpepd

=

pnpepd

. (6)

Now we examine the external forces acting on the hexa-copter. It is controlled by individually controlling the speedof the six rotors. These rotors are controlled at differentangular velocities to create varying thrusts and moments.Let the thrust of each of the individual rotors be Ti andthe moments be Mi.

The forces and moments acting on the hexacopter aregravity, individual lift forces from each propeller, the indi-vidual moments created from each motor and aerodynamicdrag forces and moments. The aerodynamic drag forces arediscussed in [13]. These are small when compared to the liftforces and moments from the propellers and therefore will be

neglected. The simple force equation in the inertial referenceframe is

x =

(1

mh

)F i, (7)

where mh is the mass of the hexacopter and F i is the externalforce in the inertial frame applied to the hexacopter. Becausethe axis of rotation of each propeller is parallel to the bodykb axis, the forces from the motors act only in the body kb

direction. These forces in the body frame are

F b =

00

−T

, (8)

where T = T1+T2+T3+T4+T5+T6, the total thrust actingon the hexacopter. The thrust in Equation (8) is negativebecause the force is acting in the upward direction and kbpoints down. Using F b rotated into the inertial frame alongwith gravity g, the dynamic equations of motion for thetranslation of the hexacopter in the inertial frame are

pnpepd

= Rib(ψ, θ, φ)

00

−T

( 1

mh

)+

00g

. (9)

Multiplying out the rotation in Equation (9) gives pnpepd

=−Tmh

cφsθcψ + sφsψcφsθsψ − sφcψ

cφcθ

+

00g

. (10)

Looking at the rotational kinematics and dynamics, therelationship between the angular position of the hexacopterand the angular rates as shown in [2] is

φ

θ

ψ

=

1 sφtθ cφtθ0 cφ −sφ0

sφcθ

cφcθ

pqr

, (11)

where tθ4= tan θ. The dynamic equations for rotational

motion as also shown in [2] are

pqr

=

Γ1pq − Γ2qrΓ5pr − Γ6

(p2 − r2

)Γ7pq − Γ1qr

+

Γ3`+ Γ4n1Jym

Γ4l + Γ8n

. (12)

The Γi terms are functions of the mass moments of inertia.The complete set of equations of motion for the hexacopterare comprised of Equations (6), (10), (11) and (12). It isimportant to note that the only assumption we have madeon the number of rotors on the vehicle is Equation (8),which can simply be changed to the number of rotors onthe rotorcraft. Other than the thrust T , there are no otherdependencies on number of rotors in this derivation.

A. State-Space Model

The hexacopter we are using comes equipped with anon-board attitude controller. The attitude controller acceptsdesired inputs T d, φd, θd and rd. The on-board attitudecontroller closes the loop on the desired input commandswhich eliminates the need to control the moments l, m, andn; accordingly we simplify the state vector as

x =[pn pe pd pn pe pd ψ

]T. (13)

We now show how we can write this system using linearequations by defining an input vector as a function of thehexacopter inputs T , φ, θ and r and the state ψ. For thisderivation we assume that the hexacopter attitude controllerwill achieve the input command with zero error, i.e., T = T d,φ = φd, θ = θd and r = rd. Define the input vector to thehexacopter as

ν =

Tφθr

.In the following discussion it will be useful to define a non-linear map of the inputs ν as follows:

u =

[up(3x1)uψ(1x1)

]4=

[fp (x,ν)fψ (ν, q)

], (14)

where

fp (x,ν) = Rib(ψ, θ, φ)

00

−T

( 1

mh

). (15)

andfψ (ν, q) = q

sinφ

cos θ+ r

cosφ

cosθ. (16)

The vector up includes the three inputs that affect pn, peand pd from the thrust rotated into the inertial frame usingEquation (9). The mapping uψ affects the heading of thevehicle. Using the mapping defined by Equations (15) and(16) we can write the dynamics in state-space form as

x = Ax+Bu+ bg, (17)

where

A =

03x3 I3x3 03x103x3 03x3 03x101x3 01x3 01x1

,B =

03x3 03x1I3x3 03x101x3 1

,and

b =[

0 0 0 0 0 1 0]T.

B. Inverse Function for Control Inputs

Equation (17) shows how the system states evolve as func-tions of the input u. The input u is in units of accelerationwhile the hexacopter inputs ν are in units force, angles andangular velocity respectively. For this reason the hexacopterinputs ν must be converted from the input u. This is done by

using the inverse functions f−1p and f−1ψ . We now show howto calculate these inverse functions. Using Equation (15), wecan solve for the first three input commands T , φ and θ. Wewrite Equation (15) as up1

up2up3

mh = R(ψ)R(θ)R(φ)

00−1

T.Taking the norm of both sides gives

T = mh

√u2p1 + u2p1 + u2p1 . (18)

Now solving for the input commands φ and θ, we furthermanipulate Equation (15) as

RT(ψ)

up1up2up3

mh

−T= R(θ)R(φ)

001

. (19)

For clarity we define

w = RT(ψ)

up1up2up3

mh

−T. (20)

Using Equations (19) and (20) we get that

φ = sin−1(w2) (21)

andθ = tan−1

(−w1

w3

), (22)

where the value wi is the ith element of w. Equations (8),(21) and (22) make up the inverse function f−1p . The lastinput r, is equal to f−1ψ and is solved from Equation (16) as

r = f−1ψ = ψ cos θ cosφ− θ sinφ, (23)

where θ is calculated from the time derivative of Equa-tion (22).

III. DIFFERENTIALLY FLAT

A differentially flat system is one in which the state andcontrol inputs can be expressed as functions of the outputand its time derivatives [8], [17], [12]. In other words,

y = h(x, u, u, u, ..., u(k)) (24)

is a flat output if there exists smooth functions gx and gusuch that

xr = gx(y, y, y, ..., y(k)) (25)

and

ur = gu(y, y, y, ..., y(k)). (26)

This implies that by specifying the output and the outputderivatives, both the reference input and the reference stateequations can be uniquely expressed as functions of thespecified output equations.

For the system the flat output vector corresponding to

Equation (24) is simply

y =

pnpepdψ

.The function gx (y, y) from Equation (25) is Equation (13).The function gu (y, y, y) is defined as follows:

gu (y, y, y) =

[urpurψ

]=

prnpreprd

−

00g

(27)

andurψ = ψ. (28)

The input ur gives us the required inputs in terms of thesystem in Equation (17), these are then converted to thehexacopter units using the inverse functions f−1p and f−1ψ .

From Equations (18), (22), (21) and (23) we now canexpress desired states and inputs as functions of any twice-differentiable desired trajectory. The limits for this approachare when the quadrotor is in free-fall and when φ = ±π

2 .Both of these scenarios can be avoided by judicious pathselection.

A. System Architecture

Fig. 3. System block diagram.

Figure 3 shows the block diagram of the system. Thetrajectory block generates a vector ytraj that consists of adesired position, velocity and acceleration along a path. TheDifferentially Flat block takes these trajectory parametersand calculates a reference input ur from Equations (27)and (28). It also calculates the reference states xr fromEquation (13). The input is the feed forward control andcontains the commands needed for the hexacopter to fly thegiven trajectory. The LQR controller takes the differencebetween the desired states xr and the actual states x toprovide the feedback control u. The reference input ur andthe feedback control input u are added together to producethe input u into the f−1 block. This input u is then used tocalculate the final input ν, which is sent to the on-boardattitude controller which is contained in the Hexacopterblock.

IV. TRAJETORY GENERATION

Any twice differentiable function can be used as a trajec-tory for the hexacopter. Four time-dependent equations arerequired, one each for pn(t), pe(t), pd(t) and ψ(t). Theseequations are then differentiated to find pn(t), pe(t), pd(t)

and ψ(t), then differentiated again to find pn(t), pe(t), pd(t)and ψ(t). These trajectory equations make up the ytraj signalin Figure 3.

We now show an example path that will be used toillustrate the procedure described above. The example path isa figure eight path with varying height and varying heading.The position trajectory of the hexacopter is given as

pnpepdψ

=

α cos(ω2 t)β sin(ωt)

−γ sin(ωt) − ηω2 t

, (29)

where α, β and γ are amplitude gains in the north, east anddown directions respectively, and where η is a bias term tokeep the desired down position from intersecting the ground.The first and second derivatives are

pnpepdψ

=

−αω sin(ω2 t)βω cos(ωt)−γω cos(ωt)

ω2

,pnpepdψ

=

−αω

2

4 cos(ω2 t)−βω2 sin(ωt)γω2 sin(ωt)

0

These are now used to compute ur. The value of ∆t is thetime for one complete lap, and is chosen to be 15 seconds forthis example. The input commands for φ(t), θ(t) and V (t)are shown in Figure 4, where V (t) is the velocity profileover the path. This velocity is calculated as

V (t) =√p2n + p2e + p2d.

0 5 10 15−5

−4

−3

−2

−1

0

1

2

3

4

5

Time (seconds)

Ang

le (

degr

ees)

RollPitchVelocity

Fig. 4. Roll and pitch commands for the figure eight path.

V. CLOSED LOOP CONTROL

The state-space model for the system is given in Equa-tion (17). We define x as the deviation of the state vectorfrom the reference state xr as

x = x− xr,

resulting in the error equations

˙x = Ax+Bu. (30)

We desire to control the system using full state feedbackwhere u = −Kx. Our goal is to design a linear quadraticregulator (LQR) controller for the hexacopter. The LQRcontroller solves for the optimal state feedback matrix Kthat minimizes [11]

J =

∫ ∞0

xTQx+ uTRu dt, (31)

where the Q and R are symmetric positive-definite weightingmatrices.

In practice, the hexacopter attitude controller will notachieve perfect tracking. For this reason we desire to add anintegrator term in the LQR controller that will improve pathtracking. To design the LQR controller with integrator wedefine a new state vector z with the following state dynamics[7]

˙z = Az + Bν, (32)

where

A =

[A 0D 0

](33)

and

B =

[B0

]. (34)

and minimize

J =

∫ ∞0

zTQzz + uTRνu dt. (35)

The final input to the system is then calculated as

u = −K1x−K2

∫ t

0

Dx(τ)dτ (36)

With this feedback control law, the closed-loop system isstable if the eigenvalues of (A− BK) are negative. For thesystem the eigenvalues are negative and the system is stablefor any input trajectory satisfying Equation (24).

VI. RESULTS

The control algorithm was tested on the MikrokopterHexacopter. The hexacopter is shown in Figure 1. Thehexacopter is flown in a room with a motion capture systemfrom Motion Analysis Corporation, which tracks the positionof the reflective spheres on the hexacopter to give inertialposition and orientation in the room.

Figure 5 shows the results of a figure eight flight ofthe hexacopter platform. The path trajectory is given inEquation (29) with α = 1, β = 0.75, γ = 0.5 and η = 1.The full loop was completed in a time of 15 seconds, witha maximum speed of 1.05 m/s. Figure 6 shows the positionerrors in the north, east and down directions for the figureeight path with constant heading. Figure 7 shows the desiredand actual positions of the hexacopter on the same figureeight path with a constant heading rate of 0.2 rad/s. Figure

Fig. 5. Figure eight path.

10 15 20 25 30 35

−0.1

−0.05

0

0.05

0.1

Time (seconds)

Err

or (

met

ers)

NorthEastDown

Fig. 6. Path errors along the figure eight path.

8 shows the desired and actual heading of the hexacopterwhile flying around the figure eight path. Figure 9 shows theoff-path error in the north, east and down directions alongthe figure eight path with the constant heading rate.

VII. CONCLUSION

We have presented a method of control for rotorcraft thatutilizes the differentially flat dynamics to generate feed-forward commands. This method greatly improves the pathfollowing control scheme. The principle and accompanyingequations are simple and easy to implement. The trajectoriesare also relatively easy to generate. It allows for aggressivemaneuvers along any smooth path with heading change. Wehave demonstrated the performance of this control schemewith hardware using a Mikrokopter hexacopter. Future workwill include an adaptive control scheme to estimate pa-rameters of the rotorcraft during flight. This will allowthe controller to compensate for inaccuracies in the vehiclemodel parameters.

ACKNOWLEDGEMENT

This research is supported in part through the DoDSMART Scholarship and under subcontract to ScientificSystems Company, Inc. as part of ONR contract numberN00014-10-M-0345.

0

0.5

1

1.5

2

−1

−0.5

0

0.5

1

1.5

Y position (meters)X position (meters)

Z p

ositi

on (

met

ers)

Actual PositionDesired Position

Fig. 7. A figure eight path with changing heading.

5 10 15 20 25 30 35

−3

−2

−1

0

1

2

3

Time (seconds)

ψ (

radi

ans)

Actual PositionDesired Position

Fig. 8. The desired and actual heading for the figure eight path.

REFERENCES

[1] S. Ahrens, D. Levine, G. Andrews, and J. P. How, “Vision-basedguidance and control of a hovering vehicle in unknown, gps-deniedenvironments,” in Proc. IEEE Int. Conf. Robotics and AutomationICRA ’09, 2009, pp. 2643–2648.

[2] R. W. Beard and T. W. McLain, Small Unmanned Aircraft. PrincetonUniversity Press, 2011.

[3] M. Blosch, S. Weiss, D. Scaramuzza, and R. Siegwart, “Vision basedmav navigation in unknown and unstructured environments,” in Proc.IEEE Int Robotics and Automation (ICRA) Conf, 2010, pp. 21–28.

[4] C. Chamberlain, “System identification, state estimation, and controlof unmanned aerial robots,” MS, Brigham Young University, Provo,UT, April 2011.

[5] H. G. W. S. T. C.J., “Quadrotor helicopter trajectory tracking control,”in Proc. AIAA Guidance, Navigation, and Control Conf., Honolulu, HI,2008.

[6] I. Cowling, O. Yakimenko, J. Whidborne, and A. Cooke, “A prototypeof an autonomous controller for a quadrotor UAV,” in Proceedings ofthe European Control Conference, 2007.

[7] P. Dorato, C. Abdallah, and V. Cerone, Linear-Quadratic Control : AnIntroduction. Prentice-Hall, 1995.

[8] M. Fliess, J. Levine, P. Martin, and P. Rouchon, “Flatness and defectof nonlinear systems: Introductory theory and examples,” CAS, Tech.Rep. A-284, January 1994.

[9] S. Grzonka, G. Grisetti, and W. Burgard, “Towards a navigation systemfor autonomous indoor flying,” in Proc. IEEE Int. Conf. Robotics andAutomation ICRA ’09, 2009, pp. 2878–2883.

[10] D. Gurdan, J. Stumpf, M. Achtelik, K.-M. Doth, G. Hirzinger, andD. Rus, “Energy-efficient autonomous four-rotor flying robot con-trolled at 1 khz,” in Proc. IEEE Int Robotics and Automation Conf,2007, pp. 361–366.

0 5 10 15 20 25 30

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

Time (seconds)

Err

or (

met

ers)

NorthEastDown

Fig. 9. Path errors along the figure eight with changing heading path.

[11] J. P. Hespanha, Linear Systems Theory. Princeton University Press,2009.

[12] P. Martin, R. Murray, and P. Rouchon, “Flat systems,” 1997.[13] P. Martin and E. Salaun, “The true role of accelerometer feedback in

quadrotor control,” in Proc. IEEE Int Robotics and Automation (ICRA)Conf, 2010, pp. 1623–1629.

[14] N. Michael, D. Mellinger, Q. Lindsey, and V. Kumar, “The graspmultiple micro-uav testbed,” IEEE Robotics & Automation Magazine,vol. 17, no. 3, pp. 56–65, 2010.

[15] Mikrokopter, “http://www.mikrokopter.de.”[16] Motion Analysis, “http://www.motionanalysis.com/.”[17] R. Murray, M. Rathinam, and W. Sluis, “Differential flatness of

mechanical control systems: A catalog of prototype systems,” in Int’lMech Eng Congress and Expo. ASME, Novermber 1995.

[18] P. C. Paul Pounds, Robert Mahony, “Modelling and control of a quad-rotor robot,” in Australasian Conference on Robotics and Automation,2006.

[19] H. Peng, J. Wu, and Q. Chen, “Modeling and control approach to aquadrotor helicopter,” in Seventh China-Japan International Workshopon Information Tech and Control, M. WU, Y. HE, J. CHEN, and J.-H.SHE, Eds. Hunan, China: Central South University China, December2009, pp. 46–55.

[20] A. Schllig, F. Augugliaro, and R. D’Andrea, “A platform for danceperformances with multiple quadrocopters,” in International Confer-ence on Intelligent Robots and Systems - Workshop on Robots andMusical Expressions, 2010, pp. 1 – 8.