quadrotor dynamics and control rev 0
TRANSCRIPT
Brigham Young University Brigham Young University
BYU ScholarsArchive BYU ScholarsArchive
Faculty Publications
2008-05-05
Quadrotor Dynamics and Control Rev 0.1 Quadrotor Dynamics and Control Rev 0.1
Randal Beard [email protected]
Follow this and additional works at: https://scholarsarchive.byu.edu/facpub
Part of the Electrical and Computer Engineering Commons
BYU ScholarsArchive Citation BYU ScholarsArchive Citation Beard, Randal, "Quadrotor Dynamics and Control Rev 0.1" (2008). Faculty Publications. 1325. https://scholarsarchive.byu.edu/facpub/1325
This Report is brought to you for free and open access by BYU ScholarsArchive. It has been accepted for inclusion in Faculty Publications by an authorized administrator of BYU ScholarsArchive. For more information, please contact [email protected], [email protected].
Quadrotor Dynamics and Control
Randal W. BeardBrigham Young University
February 19, 2008
1 Reference FramesThis section describes the various reference frames and coordinate systems thatare used to describe the position of orientation of aircraft, and the transformationbetween these coordinate systems. It is necessary to use several different coordi-nate systems for the following reasons:
• Newton’s equations of motion are given the coordinate frame attached tothe quadrotor.
• Aerodynamics forces and torques are applied in the body frame.
• On-board sensors like accelerometers and rate gyros measure informationwith respect to the body frame. Alternatively, GPS measures position,ground speed, and course angle with respect to the inertial frame.
• Most mission requirements like loiter points and flight trajectories, are spec-ified in the inertial frame. In addition, map information is also given in aninertial frame.
One coordinate frame is transformed into another through two basic opera-tions: rotations and translations. Section 1.1 develops describes rotation matricesand their use in transforming between coordinate frames. Section 1.2 describesthe specific coordinate frames used for micro air vehicle systems. In Section 1.3we derive the Coriolis formula which is the basis for transformations betweenbetween between translating and rotating frames.
1
1.1 Rotation MatricesWe begin by considering the two coordinate systems shown in Figure 1. The
Figure 1: Rotation in 2D
vector p can be expressed in both the F0 frame (specified by (i0, j0, k0)) and inthe F1 frame (specified by (i1, j1, k1)). In the F0 frame we have
p = p0xi
0 + p0y j
0 + p0zk
0.
Alternatively in the F1 frame we have
p = p1xi
1 + p1y j
1 + p1zk
1.
Setting these two expressions equal to each other gives
p1xi
1 + p1y j
1 + p1zk
1 = p0xi
0 + p0y j
0 + p0zk
0.
Taking the dot product of both sides with i1, j1, and k1 respectively, and stackingthe result into matrix form gives
p1 4=
p1x
p1y
p1z
=
i1 · i0 i1 · j0 i1 · k0
j1 · i0 j1 · j0 j1 · k0
k1 · i0 k1 · j0 k1 · k0
p0x
p0y
p0z
.
From the geometry of Figure 1 we get
p1 = R10p
0, (1)
2
where
R10
4=
cos(θ) sin(θ) 0− sin(θ) cos(θ) 0
0 0 1
.
The notation R10 is used to denote a rotation matrix from coordinate frame F0 to
coordinate frame F1.Proceeding in a similar way, a right-handed rotation of the coordinate system
about the y-axis gives
R10
4=
cos(θ) 0 − sin(θ)0 1 0
sin(θ) 0 cos(θ)
,
and a right-handed rotation of the coordinate system about the x-axis resultes in
R10
4=
1 0 00 cos(θ) sin(θ)0 − sin(θ) cos(θ)
.
As pointed out in [1], the negative sign on the sin term appears above the line withonly ones and zeros.
The matrix R10 in the above equations are examples of a more general class of
rotation matrices that have the following properties:
P.1. (Rba)−1 = (Rb
a)T = Ra
b .
P.2. RcbR
ba = Rc
a.
P.3. det Rba = 1.
In the derivation of Equation (1) note that the vector p remains constant andthe new coordinate frame F1 was obtained by rotating F0 through a righted-handed rotation of angle θ.
We will now derive a formula, called the rotation formula that performs aleft-handed rotation of a vector p about another vector n by an angle of µ. Ourderivation follows that given in [1]. Consider Figure 2 which is similar to Figure1.2-2 in [1]. The vector p is rotated, in a left-handed sense, about a unit vector nby an angle of µ to produce the vector q. The angle between p and n is φ. Bygeometry we have that
q = ~ON + ~NW + ~WQ. (2)
3
Figure 2: Left-handed rotation of a vector p about the unit vector n by an angleof µ to obtain the vector q.
The vector ~ON can be found by taking the projection of p on the unit vector n inthe direction of n:
~ON = (p · n) n.
The vector ~NW is in the direction of p− ~ON with a length of NQ cos µ. Notingthat the length NQ equals the length NP which is equal to
∥∥∥p− ~ON∥∥∥ we get
that
~NW =p− (p · n) n
‖p− (p · n) n‖NQ cos µ
= (p− (p · n) n) cos µ.
The vector ~WQ is perpendicular to both p and n and has length NQ sin µ. Notingthat NQ = ‖p‖ sin φ we get
~WQ =p× n
‖p‖ sin φNQ sin µ
= −n× p sin µ.
Therefore Equation (2) becomes
q = (1− cos µ) (p · n) n + cos µp− sin µ (n× p) , (3)
4
Figure 3: Rotation of p about the z-axis.
which is called the rotation formula.As an example of the application of Equation (3) consider a left handed rota-
tion of a vector p0 in frame F0 about the z-axis as shown in Figure 3.Using the rotation formula we get
q0 = (1− cos θ)(p · n)n + cos φp− sin φn× p
= (1− cos φ)p0z
001
+ cos φ
p0x
p0y
p0z
− sin φ
−p0
y
p0x
0
=
cos φ sin φ 0− sin φ cos φ 0
0 0 1
p0
= R10p
0.
Note that the rotation matrix R10 can be interpreted in two different ways. The
first interpretation is that it transforms the fixed vector p from an expression inframe F0 to an expression in frame F1 where F1 has been obtained from F0
by a right-handed rotation. The second interpretation is that it rotates a vectorp though a left-handed rotation to a new vector q in the same reference frame.Right-handed rotations of vectors are obtained by using (R1
0)T .
5
1.2 Quadrotor Coordinate FramesFor quadrotors there are several coordinate systems that are of interest. In thissection we will define and describe the following coordinate frames: the inertialframe, the vehicle frame, the vehicle-1 frame, the vehicle-2 frame, and the bodyframe. Throughout the book we assume a flat, non-rotating earth: a valid assump-tion for quadrotors.
1.2.1 The inertial frame F i.
The inertial coordinate system is an earth fixed coordinate system with origin atthe defined home location. As shown in Figure 4, the unit vector ii is directedNorth, ji is directed East, and ki is directed toward the center of the earth.
Figure 4: The inertial coordinate frame. The x-axis points North, the y-axis pointsEast, and the z-axis points into the Earth.
1.2.2 The vehicle frame Fv.
The origin of the vehicle frame is at the center of mass of the quadrotor. However,the axes of Fv are aligned with the axis of the inertial frame F i. In other words,the unit vector iv points North, jv points East, and kv points toward the center ofthe earth, as shown in Figure 5.
1.2.3 The vehicle-1 frame Fv1.
The origin of the vehicle-1 frame is identical to the vehicle frame, i.e, the thecenter of gravity. However, Fv1 is positively rotated about kv by the yaw angle ψso that if the airframe is not rolling or pitching, then iv1 would point out the nose
6
Figure 5: The vehicle coordinate frame. The x-axis points North, the y-axis pointsEast, and the z-axis points into the Earth.
Figure 6: The vehicle-1 frame. If the roll and pitch angels are zero, then the x-axispoints out the nose of the airframe, the y-axis points out the right wing, and thez-axis points into the Earth.
of the airframe, jv1 points out the right wing, and kv1 is aligned with kv and pointsinto the earth. The vehicle-1 frame is shown in Figure 6.
The transformation from Fv to Fv1 is given by
pv1 = Rv1v (ψ)pv,
where
Rv1v (ψ) =
cos ψ sin ψ 0− sin ψ cos ψ 0
0 0 1
.
7
1.2.4 The vehicle-2 frame Fv2.
The origin of the vehicle-2 frame is again the center of gravity and is obtained byrotating the vehicle-1 frame in a right-handed rotation about the jv1 axis by thepitch angle θ. If the roll angle is zero, then iv2 points out the nose of the airframe,jv2 points out the right wing, and kv2 points out the belly, a shown in Figure 7.
Figure 7: The vehicle-2 frame. If the roll angel is zero, then the x-axis points outthe nose of the airframe, the y-axis points out the right wing, and the z-axis pointsout the belly.
The transformation from Fv1 to Fv2 is given by
pv2 = Rv2v1(θ)p
v1,
where
Rv2v1(θ) =
cos θ 0 − sin θ0 1 0
sin θ 0 cos θ
.
1.2.5 The body frame F b.
The body frame is obtained by rotating the vehicle-2 frame in a right handedrotation about iv2 by the roll angle φ. Therefore, the origin is the center-of-gravity,ib points out the nose of the airframe, jb points out the right wing, and kb pointsout the belly. The body frame is shown in Figure 8.
The transformation from Fv2 to F b is given by
pb = Rbv2(φ)pv2,
8
Figure 8: The body frame. The x-axis points out the nose of the airframe, they-axis points out the right wing, and the z-axis points out the belly.
where
Rbv2(φ) =
1 0 00 cos φ sin φ0 − sin φ cos φ
.
The transformation from the vehicle frame to the body frame is given by
Rbv(φ, θ, ψ) = Rb
v2(φ)Rv2v1(θ)R
v1v (ψ)
=
1 0 00 cos φ sin φ0 − sin φ cos φ
cos θ 0 − sin θ0 1 0
sin θ 0 cos θ
cos ψ sin ψ 0− sin ψ cos ψ 0
0 0 1
=
cθcψ cθsψ −sθsφsθcψ − cφsψ sφsθsψ + cφcψ sφcθcφsθcψ + sφsψ cφsθsψ − sφcψ cφcθ
,
where cφ4= cos φ and sφ
4= sin φ.
1.3 Equation of CoriolisIn this section we provide a simple derivation of the famous equation of Coriolis.We will again follow the derivation given in [1].
Suppose that we are given two coordinate frames F i and F b as shown in Fig-ure 9. For example, F i might represent the inertial frame and F b might representthe body frame of a quadrotor. Suppose that the vector p is moving in F b and thatF b is rotating and translating with respect to F i. Our objective is to find the timederivative of p as seen from frame F i.
We will derive the appropriate equation through two steps. Assume first thatF b is not rotating with respect to F i. Denoting the time derivative of p in frame
9
Figure 9: Derivation of the equation of Coriolis.
F i as ddti
p we getd
dtip =
d
dtbp. (4)
On the other hand, assume that p is fixed inF b but thatF b is rotating with respectto F i, and let s be the instantaneous axis of rotation and δφ the (right-handed)rotation angle. Then the rotation formula (3) gives
p + δp = (1− cos(−δφ))s(s · p) + cos(−δφ)p− sin(−δφ)s× p.
Using a small angle approximation and dividing both sides by δt gives
δp
δt≈ δφ
δts× p.
Taking the limit as δt → 0 and defining the angular velocity of F b with respect toF i as ωb/i
4= sφ we get
d
dtip = ωb/i × p. (5)
Since differentiation is a linear operator we can combine Equations (4) and (5) toobtain
d
dtip =
d
dtbp + ωb/i × p, (6)
which is the equation of Coriolis.
10
2 Kinematics and DynamicsIn this chapter we derive the expressions for the kinematics and the dynamics of arigid body. While the expressions derived in this chapter are general to any rigidbody, we will use notation and coordinate frames that are typical in the aeronauticsliterature. In particular, in Section 2.1 we define the notation that will be used forthe state variables of a quadrotor. In Section 2.2 we derive the expressions for thekinematics, and in Section 2.3 we derive the dynamics.
2.1 Quadrotor State VariablesThe state variables of the quadrotor are the following twelve quantities
pn = the inertial (north) position of the quadrotor along ii in F i,
pe = the inertial (east) position of the quadrotor along ji in F i,
h = the altitude of the aircraft measured along −ki in F i,
u = the body frame velocity measured along ib in F b,
v = the body frame velocity measured along jb in F b,
w = the body frame velocity measured along kb in F b,φ = the roll angle defined with respect to Fv2,θ = the pitch angle defined with respect to Fv1,ψ = the yaw angle defined with respect to Fv,
p = the roll rate measured along ib in F b,
q = the pitch rate measured along jb in F b,
r = the yaw rate measured along kb in F b.
The state variables are shown schematically in Figure 10. The position (pn, pe, h)of the quadrotor is given in the inertial frame, with positive h defined along thenegative Z axis in the inertial frame. The velocity (u, v, w) and the angular veloc-ity (p, q, r) of the quadrotor are given with respect to the body frame. The Eulerangles (roll φ, pitch θ, and yaw χ) are given with respect to the vehicle 2-frame,the vehicle 1-frame, and the vehicle frame respectively.
11
Figure 10: Definition of Axes
2.2 Quadrotor KinematicsThe state variables pn, pe, and −h are inertial frame quantities, whereas the ve-locities u, v, and w are body frame quantities. Therefore the relationship betweenposition and velocities is given by
d
dt
pn
pe
−h
= Rv
b
uvw
= (Rbv)
T
uvw
=
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θ
uvw
.
The relationship between absolute angles φ, θ, and ψ, and the angular rates p,q, and r is also complicated by the fact that these quantities are defined in differentcoordinate frames. The angular rates are defined in the body frame F b, whereasthe roll angle φ is defined in Fv2, the pitch angle θ is defined in Fv1, and the yawangle ψ is defined in the vehicle frame Fv.
We need to relate p, q, and r to φ, θ, and ψ. Since φ, θ, ψ are small and notingthat
Rbv2(φ) = Rv2
v1(θ) = Rv1v (ψ) = I,
12
we get
pqr
= Rb
v2(φ)
φ00
+ Rb
v2(φ)Rv2v1(θ)
0
θ0
+ Rb
v2(φ)Rv2v1(θ)Rv→v1(ψ)
00
ψ
=
φ00
+
1 0 00 cos φ sin φ0 − sin φ cos φ
0
θ0
+
1 0 00 cos φ sin φ0 − sin φ cos φ
cos θ 0 − sin θ0 1 0
sin θ 0 cos θ
00
ψ
=
1 0 −sθ0 cφ sφcθ0 −sφ cφcθ
φ
θ
ψ
. (7)
Inverting we get
φ
θ
ψ
=
1 sin(φ) tan(θ) cos(φ) tan(θ)0 cos(φ) − sin(φ)0 sin(φ) sec(θ) cos(φ) sec(θ)
pqr
. (8)
2.3 Rigid Body DynamicsLet v be the velocity vector of the quadrotor. Newton’s laws only hold in inertialframes, therefore Newton’s law applied to the translational motion is
mdv
dti= f ,
where m is the mass of the quadrotor, f is the total applied to the quadrotor, andd
dtiis the time derivative in the inertial frame. From the equation of Coriolis we
have
mdv
dti= m
(dv
dtb+ ωb/i × v
)= f , (9)
where ωb/i is the angular velocity of the airframe with respect to the inertial frame.Since the control force is computed and applied in the body coordinate system,and since ω is measured in body coordinates, we will express Eq (9) in bodycoordinates, where vb 4
= (u, v, w)T , and ωbb/i
4= (p, q, r)T . Therefore, in body
coordinates, Eq. (9) becomes
uvw
=
rv − qwpw − ruqu− pv
+
1
m
fx
fy
fz
, (10)
13
where f b 4= (fx, fy, fz)
T .For rotational motion, Newton’s second law states that
dhb
dti= m,
where h is the angular momentum and m is the applied torque. Using the equationof Coriolis we have
dh
dti=
dh
dtb+ ωb/i × h = m. (11)
Again, Eq. (11) is most easily resolved in body coordinates where hb = Jωbb/i
where J is the constant inertia matrix given by
J =
∫(y2 + z2) dm − ∫
xy dm − ∫xz dm
− ∫xy dm
∫(x2 + z2) dm − ∫
yz dm− ∫
xz dm − ∫yz dm
∫(x2 + y2) dm
4=
Jx −Jxy −Jxz
−Jxy Jy −Jyz
−Jxz −Jyz Jz
.
Figure 11: The moments of inertia for the quadrotor are calculated assuming aspherical dense center with mass M and radius R, and point masses of mass mlocated at a distance of ` from the center.
As shown in Figure 11, the quadrotor is essentially symmetric about all threeaxes, therefore Jxy = Jxz = Jyz = 0 which implies that
J =
Jx 0 00 Jy 00 0 Jz
.
14
Therefore
J−1 =
1Jx
0 0
0 1Jy
0
0 0 1Jz
.
The inertia for a solid sphere is given by J = 2MR2/5[2]. Therefore
Jx =2MR2
5+ 2`2m
Jy =2MR2
5+ 2`2m
Jz =2MR2
5+ 4`2m.
Defining mb 4= (τφ, τθ, τψ)T we can write Eq. (11) in body coordinates as
pqr
=
1Jx
0 0
0 1Jy
0
0 0 1Jz
0 r −q−r 0 pq −p 0
Jx 0 00 Jy 00 0 Jz
pqr
+
τφ
τθ
τψ
=
Jy−Jz
Jxqr
Jz−Jx
Jypr
Jx−Jy
Jzpq
+
1Jx
τφ1Jy
τθ1Jz
τψ
.
The six degree of freedom model for the quadrotor kinematics and dynamicscan be summarized as follows:
pn
pe
h
=
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θ
uvw
(12)
uvw
=
rv − qwpw − ruqu− pv
+
1
m
fx
fy
fz
, (13)
φ
θ
ψ
=
1 sin φ tan θ cos φ tan θ0 cos φ − sin φ
0 sin φcos θ
cos φcos θ
pqr
(14)
pqr
=
Jy−Jz
Jxqr
Jz−Jx
Jypr
Jx−Jy
Jzpq
+
1Jx
τφ1Jy
τθ1Jz
τψ
. (15)
15
3 Forces and MomentsThe objective of this section is to describe the forces and torques that act on thequadrotor. Since there are no aerodynamic lifting surfaces, we will assume thatthe aerodynamic forces and moments are negligible.
The forces and moments are primarily due to gravity and the four propellers.
front
right
back
left
Figure 12: The top view of the quadrotor. Each motor produces an upward forceF and a torque τ . The front and back motors spin clockwise and the right and leftmotors spin counterclockwise.
Figure 13: Definition of the forces and torques acting on the quadrotor.
Figure 12 shows a top view of the quadrotor systems. As shown in Figure 13,each motor produces a force F and a torque τ . The total force acting on the
16
quadrotor is given byF = Ff + Fr + Fb + Fl.
The rolling torque is produced by the forces of the right and left motors as
τφ = `(Fl − Fr).
Similarly, the pitching torque is produced by the forces of the front and backmotors as
τθ = `(Ff − Fb).
Due to Newton’s third law, the drag of the propellers produces a yawing torqueon the body of the quadrotor. The direction of the torque will be in the oppositivedirection of the motion of the propeller. Therefore the total yawing torque is givenby
τψ = τr + τl − τf − τb.
The lift and drag produced by the propellers is proportional to the square of theangular velocity. We will assume that the angular velocity is directly proportionalto the pulse width modulation commend sent to the motor. Therefore, the forceand torque of each motor can be expressed as
F∗ = k1δ∗τ∗ = k2δ∗,
where k1 and k2 are constants that need to be determined experimentally, δ∗ is themotor command signal, and ∗ represents f , r, b, and l.
Therefore, the forces and torques on the quadrotor can be written in matrixform as
Fτφ
τθ
τψ
=
k1 k1 k1 k1
0 −`k1 0 `k1
`k1 0 `k1 0−k2 k2 −k2 k2
δf
δr
δb
δl
4= M
δf
δr
δb
δl
.
The control strategies derived in subsequent sections will specify forces andtorques. The actual motors commands can be found as
δf
δr
δb
δl
= M−1
Fτφ
τθ
τψ
.
17
Note that the pulse width modulation commands are required to be between zeroand one.
In addition to the force exerted by the motor, gravity also exerts a force on thequadrotor. In the vehicle frame Fv, the gravity force acting on the center of massis given by
fvg =
00
mg
.
However, since v in Equation (13) is expressed in F b, we must transform to thebody frame to give
f bg = Rb
v
00
mg
=
−mg sin θmg cos θ sin φmg cos θ cos φ
.
Therefore, equations (12)–(15) become
pn
pe
h
=
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θ
uvw
(16)
uvw
=
rv − qwpw − ruqu− pv
+
−g sin θg cos θ sin φg cos θ cos φ
+
1
m
00−F
, (17)
φ
θ
ψ
=
1 sin φ tan θ cos φ tan θ0 cos φ − sin φ
0 sin φcos θ
cos φcos θ
pqr
, (18)
pqr
=
Jy−Jz
Jxqr
Jz−Jx
Jypr
Jx−Jy
Jzpq
+
1Jx
τφ1Jy
τθ1Jz
τψ
. (19)
4 Simplified ModelsEquations (16)–(19) are the equations of motion to be used in our six degree-of-freedom simulator. However, they are not appropriate for control design for
18
several reasons. The first reason is that they are too complicated to gain significantinsight into the motion. The second reason is that the position and orientationare relative to the inertial world fixed frame, whereas camera measurements willmeasure position and orientation of the target with respect to the camera frame.
4.1 Model for estimationFor the quadrotor, we are not able to estimate the inertial position or the headingangle ψ. Rather, we will be interested in the relative position and heading of thequadrotor with respect to a ground target. The relative position of the quadrotorwill be measured in the vehicle-1 frame, i.e., the vehicle frame after it has beenrotated by the heading vector ψ. The vehicle-1 frame is convenient since x, y,and z positions are still measured relative to a flat earth, but they are vehiclecentered quantities as opposed to inertial quantities. Let px, py, and pz denotethe relative position vector between the target and the vehicle resolved in the v1frame. Therefore Eq (16) becomes
px
py
pz
=
cθ sφsθ cφsθ0 cφ −sφ−sθ sφcθ cφcθ
uvw
. (20)
4.2 Model for control designAssuming that φ and θ are small, Equation (18) can be simplified as
φ
θ
ψ
=
pqr
. (21)
Similarly, Equation (19) is simplified by assuming that the Coriolis terms qr, pr,and pq, are small to obtain
pqr
=
1Jx
τφ1Jy
τθ1Jz
τψ
. (22)
Combining Eq. (21) and (22) we get
φ
θ
ψ
=
1Jx
τφ1Jy
τθ1Jz
τψ
. (23)
19
Differentiating Eq. (16) and neglecting Rvb gives
pn
pe
pd
=
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θ
uvw
. (24)
Neglecting the Coriolis terms and plugging Eq. (17) into Eq. (24) and simplifyinggives
pn
pe
pd
=
00g
+
−cφsθcψ − sφsψ−cφsθsψ + sφcψ
−cφcθ
F
m. (25)
Therefore, the simplified inertial model is given by
pn = (− cos φ sin θ cos ψ − sin φ sin ψ)F
m(26)
pe = (− cos φ sin θ sin ψ + sin φ cos ψ)F
m(27)
pd = g − (cos φ cos θ)F
m(28)
φ =1
Jx
τφ (29)
θ =1
Jy
τθ (30)
ψ =1
Jz
τψ. (31)
(32)
The dynamics given in Equations (26)–(31) are expressed in the inertial frame.This is necessary for the simulator. However, we will be controlling position,altitude, and heading using camera frame measurements of a target position. Inthis setting heading is irrelevant. Therefore, instead of expressing the translationaldynamics in the inertial frame, we will express them in the vehicle-1 frame Fv1,which is equivalent to the inertial frame after rotating by the heading angle.
Differentiating Eq. (20) and neglecting Rv1b gives
px
py
pz
=
cθ sφsθ cφsθ0 cφ −sφ−sθ sφcθ cφcθ
uvw
. (33)
20
Neglecting the Coriolis terms and plugging Eq. (17) into Eq. (33) and simplifyinggives
px
py
pz
=
00g
+
−cφsθ
sφ−cφcθ
F
m. (34)
Therefore, the simplified model in the vehicle-1 frame is given by
px = − cos φ sin θF
m(35)
py = sin φF
m(36)
pz = g − cos φ cos θF
m(37)
φ =1
Jx
τφ (38)
θ =1
Jy
τθ (39)
ψ =1
Jz
τψ. (40)
(41)
5 Sensors
5.1 Rate GyrosA MEMS rate gyro contains a small vibrating lever. When the lever undergoesan angular rotation, Coriolis effects change the frequency of the vibration, thusdetecting the rotation. A brief description of the physics of rate gyros can befound at RateSensorAppNote.pdf.
The output of the rate gyro is given by
ygyro = kgyroΩ + βgyro + ηgyro,
where ygyro is in Volts, kgyro is a gain, Ω is the angular rate in radians per second,βgyro is a bias term, and ηgyro is zero mean white noise. The gain kgyro should begiven on the spec sheet of the sensor. However, due to variations in manufacturingit is imprecisely known. The bias term βgyro is strongly dependent on temperatureand should be calibrated prior to each flight.
21
If three rate gyros are aligned along the x, y, and z axes of the quadrotor, thenthe rate gyros measure the angular body rates p, q, and r as follows:
ygyro,x = kgyro,xp + βgyro,x + ηgyro,x
ygyro,y = kgyro,yq + βgyro,y + ηgyro,y
ygyro,z = kgyro,zr + βgyro,z + ηgyro,z.
MEMS gyros are analog devices that are sampled by the on-board processer.We will assume that the sample rate is given by Ts. The Kestrel autopilot samplesthe gyros at approximately 120 Hz.
5.2 AccelerometersA MEMS accelerometer contains a small plate attached to torsion levers. Theplate rotates under acceleration and changes the capacitance between the plateand the surrounding walls [3].
The output of the accelerometers is given by
yacc = kaccA + βacc + ηacc,
where yacc is in Volts, kacc is a gain, A is the acceleration in meters per second, βacc
is a bias term, and ηacc is zero mean white noise. The gain kacc should be givenon the spec sheet of the sensor. However, due to variations in manufacturing it isimprecisely known. The bias term βacc is strongly dependent on temperature andshould be calibrated prior to each flight.
Accelerometers measure the specific force in the body frame of the vehicle. Aphysically intuitive explanation is given in [1, p. 13-15]. Additional explanationis given in [4, p. 27]. Mathematically we have
ax
ay
az
=
1
m(F− Fgravity)
= v + ωbb/i × v − 1
mFgravity.
In component form we have
ax = u + qw − rv + g sin θ
ay = v + ru− pw − g cos θ sin φ
az = w + pv − qu− g cos θ cos φ.
22
Using Eq (17) we get
ax = 0
ay = 0
az = −F/m,
where F is the total thrust produced by the four motors. It is important to notethat for the quadrotor, the output of the accelerometers is independent of angle.This is in contrast to the unaccelerated flight for fixed wing vehicles where theaccelerometers are used to measure the gravity vector and thereby extract roll andpitch angles.
MEMS accelerometers are analog devices that are sampled by the on-boardprocesser. We will assume that the sample rate is given by Ts. The Kestrel autopi-lot samples the accelerometers at approximately 120 Hz.
5.3 CameraThe control objective is to hold the position of the quadrotor over a ground basedtarget that is detected using the vision sensor. In this section we will briefly de-scribe how to estimate px and py in the vehicle 1-frame.
We will assume that the camera is mounted so that the optical axis of thecamera is aligned with the body frame z-axis and so that the x-axis of the camerapoints out the right of the quadrotor and the y-axis of the camera points to theback of the quadrotor.
The camera model is shown in Figure 14. The position of the target in thevehicle-1 frame is (px, py, pz). The pixel location of the target in the image is(εx, εy).
The geometry for py is shown in Figure 15. From the geometry shown inFigure 15, we can see that
py = pz tan
(φ− εx
η
My
), (42)
where η is the camera field-of-view, and My is the number of pixels along thecamera y-axis. In Figure 15, both py and εx are negative. Positive values aretoward the right rotor. A similar equation can be derived for px as
px = −pz tan
(θ − εy
η
My
). (43)
23
Figure 14: Camera model for the quadrotor.
6 State EstimationThe objective of this section is to describe techniques for estimating the state of thequadrotor from sensor measurements. We need to estimate the following states:px, py, pz, u, v, w, φ, θ, ψ, p, q, r.
The angular rates p, q, and r can be obtained by low pass filtering the rategyros. The remain states require a Kalman filter. Both are discussed below.
6.1 Low Pass FiltersThe Laplace transforms representation of a simple low-pass filter is given by
Y (s) =a
s + aU(s),
24
Target
OpticalAxis
Figure 15: The geometry introduced by the vision system. The height aboveground is given by −pz, the lateral position error is py, the roll angle is φ, thefield-of-view of the camera is η, the lateral pixel location of the target in the imageis εx, and the total number of pixels along the lateral axis of the camera is Mx.
were u(t) is the input of the filter and y(t) is the output. Inverse Laplace trans-forming we get
y = −ay + au. (44)
Using a zeroth order approximation of the derivative we get
y(t + T )− y(t)
T= −ay(t) + au(t),
where T is the sample rate. Solving for y(t + T ) we get
y(t + T ) = (1− aT )y(t) + aTu(t).
For the zeroth order approximation to be valid we need aT ¿ 1. If we let α = aTthen we get the simple form
y(t + T ) = (1− α)y(t) + αu(t).
25
Note that this equation has a nice physical interpretation: the new value of y(filtered value) is a weighted average of the old value of y and u (unfiltered value).If u is noisy, then α ∈ [0, 1] should be set to a small value. However, if u isrelatively noise free, then α should be close to unity.
In the derivation of the discrete-time implementation of the low-pass filter, it ispossible to be more precise. In particular, returning to Equation (44), from linearsystems theory, it is well known that the solution is given by
y(t + T ) = e−aT y(t) + a
∫ T
0
e−a(T−τ)u(τ) dτ.
Assuming that u(t) is constant between sample periods results in the expression
y(t + T ) = e−aT y(t) + a
∫ T
0
e−a(T−τ) dτu(t)y(t + T )
= e−aT y(t) + (1− e−aT )u(t). (45)
Note that since ex = 1+x+ x2
2!+ . . . we have that e−aT ≈ 1−aT and 1−e−aT ≈
aT .Therefore, if the sample rate is not fixed (common for micro controllers), and
it is desired to have a fixed cut-off frequency, then Equation (45) is the preferableway to implement a low-pass filter in digital hardware.
We will use the notation LPF (·) to represent the low-pass filter operator.Therefore x = LPF (x) is the low-pass filtered version of x.
6.2 Angular Rates p, q, and r.The angular rates p, q, and r can be estimated by low-pass filtering the rate gyrosignals:
p = LPF (ygyro,x) (46)q = LPF (ygyro,y) (47)r = LPF (ygyro,z). (48)
6.3 Dynamic Observer TheoryThe objective of this section is to briefly review observer theory.
26
Suppose that we have a linear time-invariant system modeled by the equations
x = Ax + Bu
y = Cx.
A continuous-time observer for this system is given by the equation˙x = Ax + Bu︸ ︷︷ ︸ + L (y − Cx)︸ ︷︷ ︸, (49)
copy of the model correction due to sensor reading
where x is the estimated value of x. Letting x = x− x we observer that
˙x = (A− LC)x
which implies that the observation error decays exponentially to zero if L is cho-sen such that the eigenvalues of A − LC are in the open left half of the complexplane.
In practice, the sensors are usually sampled and processed in digital hardwareat some sample rate Ts. How do we modify the observer equation shown in Equa-tion (49) to account for sampled sensor readings?
The typical approach is to propagate the system model between samples usingthe equation
˙x = Ax + Bu (50)
and then to update the estimate when a measurement is received using the equation
x+ = x− + L(y(tk)− Cx−),
where tk is the instant in time that the measurement is received and x− is thestate estimate produced by Equation (50) at time tk. Equation (50) is then re-instantiated with initial conditions given by x+. The continuous-discrete observeris summarized in Table 6.3.
The observation process is shown graphically in Figure 16. Note that it is notnecessary to have a fixed sample rate. The continuous-discrete observer can beimplemented using Algorithm 1.
Note that we did not use the fact that the process was linear. Suppose insteadthat we have a nonlinear system of the form
x = f(x, u) (51)y = c(x) (52)
, then the continuous discrete observer is given in table 6.3.The real question is how to pick the observer gain L.
27
Figure 16: sdf
Algorithm 1 Continuous-Discrete Observer1: Initialize: x = 0.2: Pick an output sample rate Tout which is much less than the sample rates of
the sensors.3: At each sample time Tout:4: for i = 1 to N do Prediction: Propagate the state equation.5: x = x +
(Tout
N
)(Ax + Bu)
6: end for7: if A measurement has been received from sensor i then Correction: Mea-
surement Update8: x = x + Li (yi − Cix)9: end if
28
System model:x = Ax + Buy(tk) = Cx(tk)Initial Condition x(0).
Assumptions:Knowledge of A, B, C, u(t).No measurement noise.
Prediction: In between measurements (t ∈ [tk−1, tk)):Propagate ˙x = Ax + Bu.Initial condition is x+(tk−1).Label the estimate at time tk as x−(tk).
Correction: At sensor measurement (t = tk):x+(tk) = x−(tk) + L (y(tk)− Cx−(tk)) .
Table 1: Continuous-Discrete Linear Observer.
6.4 Essentials from Probability TheoryLet X = (x1, . . . , xn)T be a random vector whose elements are random variables.The mean, or expected value of X is denoted by
µ =
µ1...
µn
=
Ex1...
Exn
= EX,
whereExi =
∫ξfi(ξ) dξ,
and f(·) is the probability density function for xi. Given any pair of componentsxi and xj of X , we denote their covariance as
cov(xi, xj) = Σij = E(xi − µi)(xj − µj).The covariance of any component with itself is the variance, i.e.,
var(xi) = cov(xi, xi) = Σii = E(xi − µi)(ξ − µi).The standard deviation of xi is the square root of the variance:
stdev(xi) = σi =√
Σii.
29
System model:x = f(x, u)y(tk) = c(x(tk))Initial Condition x(0).
Assumptions:Knowledge of f , c, u(t).No measurement noise.
Prediction: In between measurements (t ∈ [tk−1, tk)):Propagate ˙x = f(x, u).Initial condition is x+(tk−1).Label the estimate at time tk as x−(tk).
Correction: At sensor measurement (t = tk):x+(tk) = x−(tk) + L (y(tk)− c(x−(tk))) .
Table 2: Continuous-Discrete Nonlinear Observer.
The covariances associated with a random vector X can be grouped into a matrixknown as the covariance matrix:
Σ =
Σ11 Σ12 · · · Σ1n
Σ21 Σ22 · · · Σ2n... . . . ...
Σn1 Σn2 · · · Σnn
= E(X − µ)(X − µ)T = EXXT − µµT .
Note that Σ = ΣT so that Σ is both symmetric and positive semi-definite, whichimplies that its eigenvalues are real and nonnegative.
The probability density function for a Gaussian random variable is given by
fx(x) =1√
2πσx
e− (x−µx)2
σ2x ,
where µx is the mean of x and σx is the standard deviation. The vector equivalentis given by
fX(X) =1√
2π det Σexp
[−1
2(X − µ)T Σ−1(X − µ)
],
in which case we writeX ∼ N (µ, Σ) ,
30
Figure 17: Level curves for the pdf of a 2D Gaussian random variable.
and say that X is normally distributed with mean µ and covariance Σ.Figure 17 shows the level curves for a 2D Gaussian random variable with
different covariance matrices.
6.5 Derivation of the Kalman FilterIn this section we assume the following state model:
x = Ax + Bu + Gξ
yk = Cxk + ηk,
where yk = y(tk) is the kth sample of y, xk = x(tk) is the kth sample of x, ηk isthe measurement noise at time tk, ξ is a zero-mean Gaussian random process withcovariance Q, and ηk is a zero-mean Gaussian random variable with covarianceR. Note that the sample rate does not need to be be fixed.
The observer will therefore have the following form:
31
Prediction: In between measurements: (t ∈ [tk−1, tk])
Propagate ˙x = Ax + Bu.Initial condition is x+(tk−1).Label the estimate at time tk as x−(tk).
Correction: At sensor measurement (t = tk):x+(tk) = x−(tk) + L (y(tk)− Cx−(tk)) .
Our objective is to pick L to minimize tr(P (t)).
6.5.1 Between Measurements.
Differentiating x we get
˙x = x− ˙x
= Ax + Bu + Gξ − Ax−Bu
= Ax + Gξ.
Therefore we have that
x(t) = eAtx0 +
∫ t
0
eA(t−τ)Gξ(τ) dτ.
We can therefore compute the evolution for P as
P =d
dtExxT
= E ˙xxT + x ˙xT= E
AxxT + GξxT + xxT AT + xξT GT
= AP + PAT + GEξxTT + ExξTGT .
As in the previous section we get
EξxT = E
ξ(t)x0e
AT t +
∫ t
0
ξ(t)ξT (τ)GT eAT (t−τ) dτ
=1
2QGT ,
which implies thatP = AP + PAT + GQGT .
32
6.5.2 At Measurements.
At a measurement we have that
x+ = x− x+
= x− x− − L(Cx + η − Cx−
)
= x− − LCx− − Lη.
Therefore
P+ = Ex+x+T= E
(x− − LCx− − Lη
) (x− − LCx− − Lη
)T
= Ex−x−T − x−x−T CT LT − x−ηT LT
− LCx−x−T + LCx−x−T CT LT + LCx−ηT LT
= −Lηx−T + Lηx−T CT LT + LηηT LT
= P− − P−CT LT − LCP− + LCP−CT LT + LRLT . (53)
Our objective is to pick L to minimize tr(P+). A necessary condition is
∂
∂Ltr(P+) = −P−CT − P−CT + 2LCP−CT + 2LR = 0
=⇒ 2L(R + CP−CT ) = 2P−CT
=⇒ L = P−CT (R + CP−CT )−1.
Plugging back into Equation (53) give
P+ = P− + P−CT (R + CP−CT )−1CP− − P−CT (R + CP−CT )−1CP−
+ P−CT (R + CP−CT )−1(CP−CT + R)(R + CP−CT )−1CP−
= P− − P−CT (R + CP−CT )−1CP−
= (I − P−CT (R + CP−CT )−1C)P−
= (I − LC)P−.
For linear systems, the continuous-discrete Kalman filter is summarized inTable 6.5.2
If the system is nonlinear, then the Kalman filter can still be applied but weneed to linearize the nonlinear equations in order to compute the error covariancematrix P and the Kalman gain L. The extended Kalman filter (EKF) is given inTable 6.5.2, and an algorithm to implement the EKF is given in Algorithm 2.
33
System model:x = Ax + Bu + ξyi(tk) = Cix(tk) + ηk
Initial Condition x(0).Assumptions:
Knowledge of A, B, Ci, u(t).Process noise satisfies ξ ∼ N (0, Q).Measurement noise satisfies ηk ∼ N (0, R).
Prediction: In between measurements (t ∈ [tk−1, tk)):Propagate ˙x = Ax + Bu.Propagate P = AP + PAT + Q.
Correction: At the ith sensor measurement (t = tk):Li = P−CT
i (Ri + CiPCTi )−1,
P+ = (I − LiCi)P−,
x+(tk) = x−(tk) + L (y(tk)− Cx−(tk)) .
Table 3: Continuous-Discrete Kalman Filter.
6.6 Application to the quadrotorIn this section we will discuss the application of Algorithm 2 to the quadrotor.
We would like to estimate the state
x =
px
py
pz
˙px
˙py
˙pz
φ
θ
ψ
,
where the rate gyros and accelerometers will be used to drive the prediction step,and an ultrasonic altimeter and camera will be used in the correction step.
34
System model:x = f(x, u) + ξyi(tk) = ci(x(tk)) + ηk
Initial Condition x(0).Assumptions:
Knowledge of f , ci, u(t).Process noise satisfies ξ ∼ N (0, Q).Measurement noise satisfies ηk ∼ N (0, R).
Prediction: In between measurements (t ∈ [tk−1, tk)):Propagate ˙x = f(x, u),Compute A = ∂f
∂x
∣∣x=x(t)
,Propagate P = AP + PAT + Q.
Correction: At the ith sensor measurement (t = tk):Ci = ∂ci
∂x
∣∣x=x−
Li = P−CTi (Ri + CiPCT
i )−1,P+ = (I − LiCi)P
−,x+(tk) = x−(tk) + L (y(tk)− ci(x
−(tk))) .
Table 4: Continuous-Discrete Extended Kalman Filter.
The propagation model is obtained from Equations (35)–(37), and (18) as
f(x, u) =
px
py
pz
cos φ sin θaz
−sinφaz
g + cos φ cos θaz
p + q sin φ tan θ + r cos φ tan θq cos φ− r sin φ
q sin φcos θ
+ r cos φcos θ
,
where we have used the fact that the z-axis of the accelerometer measures az =
35
Algorithm 2 Continuous-Discrete Extended Kalman Filter1: Initialize: x = 0.2: Pick an output sample rate Tout which is much less than the sample rates of
the sensors.3: At each sample time Tout:4: for i = 1 to N do Prediction: Propagate the equations.5: x = x +
(Tout
N
)(f(x, u))
6: A = ∂f∂x
7: P = P +(
Tout
N
) (AP + PAT + GQGT
)8: end for9: if A measurement has been received from sensor i then Correction: Mea-
surement Update10: Ci = ∂ci
∂x
11: Li = PCTi (Ri + CiPCT
i )−1
12: P = (I − LiCi)P13: x = x + Li (yi − ci(x)).14: end if
−F/m. Differentiating we obtain
A =
0 0 0 1 0 0 0 0 00 0 0 0 1 0 0 0 00 0 0 0 0 1 0 0 00 0 0 0 0 0 −sφsθaz cφcθaz 00 0 0 0 0 0 −cφaz 0 00 0 0 0 0 0 −sφcθaz −cφsθaz 00 0 0 0 0 0 qcφtθ − rsφtθ (qsφ + rcφ)/c2θ 00 0 0 0 0 0 −qsφ− rcφ 0 0
0 0 0 0 0 0 qcφ−rsφcθ
−(qsφ + rcφ) tθcθ
0
Note that it may be adequate (not sure) to use a small angle approximation in
36
the model resulting in
f(x, u) =
px
py
pz
θaz
−φaz
g + az
pqr
,
and
A =
0 0 0 1 0 0 0 0 00 0 0 0 1 0 0 0 00 0 0 0 0 1 0 0 00 0 0 0 0 0 0 az 00 0 0 0 0 0 −az 0 00 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0
.
If this form works, then the update equation for P can be coded by hand, signif-icantly reducing the computational burden. Note also that f(x, u) does not takeinto account the motion of the target. A feedforward term can be added to f toaccount for the target motion as
f(x, u) =
px − mx
py − my
pz
θaz − mx
−φaz − my
g + az
pqr
,
where (mx, my) is the velocity of the target in the vehicle-1 frame, and (mx, my)is the acceleration of the target in the vehicle-1 frame.
37
Let the relative position between the quadrotor and the target be denoted bypv1 = (px, py, pz)
T . We can tranform to the camera frame as
pc = RcgR
gbR
bv1p
v1,
where
Rcg =
0 1 00 0 11 0 0
is the rotation matrix from gimbal coordinates to camera coordinates,
Rgb =
0 0 10 1 0−1 0 0
transforms body coordinates to gimbal coordinates, and
Rbv1 =
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θ
transforms vehicle-1 coordinates to body coordinates. Using the small angle ap-proximation for φ and θ we get
pc =
φθpx + py + φpz
−px + θpz
θpx − φpy + pz
.
The model for the pixel coordinates are therefore computed as
εx = cεx(x)4= f
φθpx + py + φpz
θpx − φpy + pz
ε2 = cεy(x)4= f
−px + θpz
θpx − φpy + pz
.
We therefore have the following sensors available to correct the state estimate:
sensor 1: altimeter y1 = calt(x)4= −pz + η1[k] (54)
sensor 2: vision x-pixel y2 = cεx(x) + η2[k]. (55)sensor 3: vision y-pixel y3 = cεy(x) + η3[k] (56)
sensor 4: vision heading y4 = cεψ(x)
4= π/2 + ψ + η4[k]. (57)
38
The linearization of the first and fourth output functions are given by
C1 =(0 0 −1 0 0 0 0 0
)(58)
C4 =(0 0 0 0 0 0 0 1
). (59)
The linearization of the expression for the pixel coordinates is messy and caneasily be computed numerically using the approximation
∂f(x1, · · · , xi,. . . , xn)
∂xi
≈ f(x1, · · · , xi + ∆, · · · , xn)− f(x1, · · · , xi, · · · , xn)
∆,
where ∆ is a small constant.
7 Control DesignThe control design will be derived directly from Equations (35)–(40). Equa-tions (38)–(40) are already linear. To simplify Equations (35)–(37) define
ux4= −cosφ sin θ
F
m(60)
uy4= sin φ
F
m(61)
uz4= g − cos φ cos θ
F
m, (62)
to obtain
px = ux (63)py = uy (64)pz = uz. (65)
The control design proceeds by developing PID control strategies for ux, uy, anduz. After ux, uy, and uz have been computed, we can compute the desired forceF , and the commanded roll angle φc and the commanded pitch angle θc fromEquations (60)–(62) as follows. From Equation (62) solve for F/m as
F
m=
g − uz
cos φ cos θ. (66)
39
Substituting (66) into (61) gives
uy =g − uz
cos θtan φ.
Solving for φ and letting this be the commanded roll angle gives
φc = tan−1
(uy cos θ
g − uz
). (67)
Similarly, we can solve for the commanded pitch angle as
θc = tan−1
(ux
uz − g
). (68)
7.1 Vision Based Altitude HoldFor the altitude hold we need to develop an expression for uz to drive pz to adesired altitude based on the size of the object in the image. We will assume thatthe camera returns the size of the object in the image plane in pixels, which isdenoted by εs. Figure 18 shows the geometry of the quadrotor hovering over a
Target
Figure 18: The size of the target is S in meters, and the size of the target in theimage plane is denoted by εs in pixels. The focal length is f , and the height abovethe target is −pz.
target of size S. From similar triangles we have that
−pz
S=
f
εs
.
40
Solving for pz an differentiating we obtain
pz =fSεs
ε2s
. (69)
Differentiating again gives
pz =fSεs
ε2s
− 2fSε2s
ε3s
.
Substituting uz = pz and solving for εs gives
εs =
(ε2s
fS
)uz + 2
ε2s
εs
.
Defining
us4=
(ε2s
fS
)uz + 2
ε2s
εs
, (70)
we getεs = us.
We can now derive a PID controller to drive εs → εds as
us = kps(εds − εs)− kds εs + kis
∫ t
0
(εds − εs) dτ.
Solving (70) for uz we get
uz =fS
ε2s
kps(εds − εs)−
(kds
fS
ε2s
+ 2fSεs
ε3s
)εs + kis
fS
ε2s
∫ t
0
(εds − εs) dτ. (71)
The downside to this equation is that it requires knowledge of the target size S andthe focal length f . This requirement can be removed by incorporating fS into thePID gains by defining
kps
4= fSkps
kis
4= fSkis
kds
4= fSkds ,
and by noting from Equation (69) that fSεs
ε2s= w. Therefore Equation (71) be-
comes
uz = kps
(εds − εs)
ε2s
−(
kds
ε2s
+ 2w
εs
)εs +
kis
ε2s
∫ t
0
(εds − εs) dτ. (72)
41
7.2 Roll Attitude HoldThe equation of motion for roll is given by Eq. (38) as φ = τp/Jx. We will use aPID controller to regulate the roll attitude as
τp = kpφ(φc − φ)− kdφ
p + kiφ
∫ t
0
(φc − φ) dτ.
A block diagram of the control structure is shown in Figure 19.
Figure 19: A block diagram of the roll attitude hold loop.
The gains kpφ, kiφ , and kdφ
are selected one at a time using a method calledsuccessive loop closure. To pick kpφ
note that if the input command φc is a step ofvalue A, then at time t = 0, before the integrator has time to begin winding up, τx
is given byτx(0) = kpφ
A.
Therefore, setting kpφ= M/A will just saturate the actuators when a step value
of A is placed on φc.To select kdφ
, let kpφbe fixed and let kiφ = 0, and solve for the characteristic
equation in Evan’s form verses kdφto obtain
1 + kdφ
1Jx
s
s2 +kpφ
Jx
= 0.
The derivative gain kdφis selected to achieve a damping ratio of ζ = 0.9.
The characteristic equation including kiφ in Evan’s form verses kiφ is given by
1 + kiφ
1Jx
s3 +kdφ
Jxs2 +
kpφ
Jxs
= 0.
42
The integral gain kiφ can be selected so that damping ratio is not significantlychanged.
7.3 Pitch Attitude HoldThe equation of motion for pitch is given by Eq. (39) as θ = τq/Jy. Similar to theroll attitude hold, we will use a PID controller to regulate pitch as
τq = kpθ(θc − θ)− kdθ
q + kiθ
∫ t
0
(θc − θ) dτ.
7.4 Vision Based Position TrackingFrom Equation 64 the lateral dynamics are given by py = uy, where py is therelative lateral position which we drive to zero using the PID strategy
uy = −kpypy − kdyv + kiy
∫ t
0
py dτ.
The relative position error py is given by Equation (42).From Equation 63 the longitudinal dynamics are given by px = ux, where px
is the relative lateral position which we drive to zero using the PID strategy
ux = −kpxpx − kdxu + kix
∫ t
0
px dτ.
The relative position error px is given by Equation (43).
7.5 Relative Heading HoldThe heading dynamics are given in Equation (40) as ψ = τr/Jz. Define ψd as theinertial heading of the target, and define ψ
4= ψ − ψd as the relative heading. The
camera directly measures ψ. Assuming that ψd is constant we get ¨ψ = τr/Jz. Weregulate the relative heading with the PID strategy
τr = −kpψψ − kdψ
r − kiψ
∫ t
0
ψ dτ.
43
7.6 FeedforwardWhen the quadrotor is tracking a ground robot, the motion of the robot will causetracking errors due to the delayed response of the PID controller. If we can com-municate with the robot, and we know its intended motion, we should be able touse that information to help the quadrotor predict where the robot is moving.
To motivate our approach, lets consider a simple example to gain intuition.Consider the double integrator system
y = u,
where y is position, and u is commanded acceleration. If r is the reference signalthen the standard PD loop is shown in Figure 20.
Figure 20: Standard PD loop
Let e = r − y, then
e = r − y
= r − u
= r − kpe + kdy
= r − kpe− kde + kdr.
In other words we have
e + kde + kpe = r + kdr.
Therefore, the signal r + kdr drives the error transfer function 1s2+kds+kp
and if wis not zero, the tracking error will not go to zero.
44
From the expression e = r − u we see that if instead of u = kpe − kdy, weinstead use
u = r + kpe + kde,
then we gete + kde + kpe = 0,
which ensures that e(t) → 0 independent of r(t). The associated block diagramis shown in Figure 21.
Figure 21: Feedforward term is added to a standard PD loop.
Now consider the lateral motion of the quadrotor in the cage where
y = g tan φ,
where y is the position of the quadrotor, g is the gravity constant, and φ is the rollangle. Let m(t) be the position of the target and define y
4= m− r as the relative
position. Then
y = m− r
= m− g tan φ.
If by some means we know the target velocity m and the target acceleration m,then we should pick φ such that
g tan φ = m + kpy + kdy,
or
φ = tan−1
(m + kpy + kdy
g
).
45
Given the decoupling terms described at the beginning of this section, themotion model for the quadrotor in the vehicle-1 frame is given by
px = ux
py = uy
ψ = uψ.
Let the position of the target in the vehicle-1 frame be given by (mx,my) and letit orientation be given by ψt, and define
px = mx − px
py = my − py
ψ = ψt − ψ.
Differentiating twice we get
¨px = mx − ux
¨py = my − uy
¨ψ = ψt − uψ.
Therefore, adding a feedforward term we have
ux = mx + PIDx
uy = my + PIDy
uψ = ψt + PIDψ
where PID∗ are the PID controllers derived earlier in this chapter.The motion model for the target is given by
mn = Vt cos ψt
me = Vt sin ψt
Vt = u1
ψt = u2,
where (mn,me) is the inertial position of the target, Vt is the ground speed of thetarget, and u1 and u2 are robot control signals. In the vehicle-1 frame we have
(mx
my
)=
(cos ψ − sin ψsin ψ cos ψ
)(Vt cos ψt
Vt sin ψt
)=
(Vt cos ψ
Vt sin ψ
),
46
where the camera measures the relative heading ψ = ψ − ψt. Differentiating weobtain (
mx
my
)=
(cos ψ sin ψ
− sin ψ cos ψ
)(u1
u2 − ψ
).
To implement feedforward on the quadrotors, we must communicate with therobot to obtain Vt, u1, and u2.
References[1] B. L. Stevens and F. L. Lewis, Aircraft Control and Simulation. Hoboken, New Jersey:
John Wiley & Sons, Inc., 2nd ed., 2003.
[2] D. Halliday and R. Resnick, Fundamentals of Physics. John Wiley & Sons, 3rd ed.,1988.
[3] http://www.silicondesigns.com/tech.html.
[4] M. Rauw, FDC 1.2 - A SIMULINK Toolbox for Flight Dynamics and Control Analy-sis, February 1998. Available at http://www.mathworks.com/.
47