Best documentation for quadrotor for deriving EOMs and control algorithms

© All Rights Reserved

0 tayangan

Best documentation for quadrotor for deriving EOMs and control algorithms

© All Rights Reserved

- Physics First Year
- 6FX2001-4DA10 Datasheet En
- Gyro submarino_Octans3000.pdf
- 10-EASY-ROB-Chapter-08
- Mission Critical Winter 2012
- INS Algorithms
- cruise messile working
- Sample Problems
- Rakesh IJAMM 1
- Analysis of the Electrolytic Codeposition of Non-Brownian P
- Railway
- Multi-uav Int Nav With Vision
- Balancing Pump Impellers
- 975956
- DH-deriv
- MAPSeqns Updated
- Ijcas SEO 2006
- 2009 FerreiraPintoMatosCruz b
- crisanbuckling
- pg1017e

Anda di halaman 1dari 47

Randal W. Beard

Brigham Young University

February 19, 2008

1 Reference Frames

This section describes the various reference frames and coordinate systems that

are used to describe the position of orientation of aircraft, and the transformation

between these coordinate systems. It is necessary to use several different coordi-

nate systems for the following reasons:

the quadrotor.

with 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 an

inertial frame.

One coordinate frame is transformed into another through two basic opera-

tions: rotations and translations. Section 1.1 develops describes rotation matrices

and their use in transforming between coordinate frames. Section 1.2 describes

the specific coordinate frames used for micro air vehicle systems. In Section 1.3

we derive the Coriolis formula which is the basis for transformations between

between between translating and rotating frames.

1

1.1 Rotation Matrices

We begin by considering the two coordinate systems shown in Figure 1. The

Figure 1: Rotation in 2D

the F 1 frame (specified by (î1 , ĵ 1 , k̂ 1 )). In the F 0 frame we have

p = p0x î0 + p0y ĵ 0 + p0z k̂ 0 .

Alternatively in the F 1 frame we have

p = p1x î1 + p1y ĵ 1 + p1z k̂ 1 .

Setting these two expressions equal to each other gives

p1x î1 + p1y ĵ 1 + p1z k̂ 1 = p0x î0 + p0y ĵ 0 + p0z k̂ 0 .

Taking the dot product of both sides with î1 , ĵ 1 , and k̂ 1 respectively, and stacking

the result into matrix form gives

1 1 0 1 0 1 0 0

px î · î î · ĵ î · k̂ px

1 4 1

1 0 1

p = py = ĵ · î ĵ · ĵ ĵ · k̂ 0 1 0 p0y .

1

pz k̂ 1 · î0 k̂ 1 · ĵ 0 k̂ 1 · k̂ 0 p0z

p1 = R01 p0 , (1)

2

where

cos(θ) sin(θ) 0

4

R01 = − sin(θ) cos(θ) 0 .

0 0 1

The notation R01 is used to denote a rotation matrix from coordinate frame F 0 to

coordinate frame F 1 .

Proceeding in a similar way, a right-handed rotation of the coordinate system

about the y-axis gives

cos(θ) 0 − sin(θ)

4

R01 = 0 1 0 ,

sin(θ) 0 cos(θ)

and a right-handed rotation of the coordinate system about the x-axis resultes in

1 0 0

4

R01 = 0 cos(θ) sin(θ) .

0 − sin(θ) cos(θ)

As pointed out in [1], the negative sign on the sin term appears above the line with

only ones and zeros.

The matrix R01 in the above equations are examples of a more general class of

rotation matrices that have the following properties:

In the derivation of Equation (1) note that the vector p remains constant and

the new coordinate frame F 1 was obtained by rotating F 0 through a righted-

handed rotation of angle θ.

We will now derive a formula, called the rotation formula that performs a

left-handed rotation of a vector p about another vector n̂ by an angle of µ. Our

derivation follows that given in [1]. Consider Figure 2 which is similar to Figure

1.2-2 in [1]. The vector p is rotated, in a left-handed sense, about a unit vector n̂

by an angle of µ to produce the vector q. The angle between p and n̂ is φ. By

geometry we have that

q = ON ~ + N~W + W~Q. (2)

3

Figure 2: Left-handed rotation of a vector p about the unit vector n̂ by an angle

of µ to obtain the vector q.

The vector ON~ can be found by taking the projection of p on the unit vector n̂ in

the direction of n̂:

~ = (p · n̂) n̂.

ON

The vector N~W is in the direction of p − ON

~ with a length of N Q cos µ. Noting

° °

° ~ °

that the length N Q equals the length N P which is equal to °p − ON ° we get

that

p − (p · n̂) n̂

N~W = N Q cos µ

kp − (p · n̂) n̂k

= (p − (p · n̂) n̂) cos µ.

The vector W~Q is perpendicular to both p and n̂ and has length N Q sin µ. Noting

that N Q = kpk sin φ we get

p × n̂

W~Q = N Q sin µ

kpk 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.

As an example of the application of Equation (3) consider a left handed rota-

tion of a vector p0 in frame F 0 about the z-axis as shown in Figure 3.

Using the rotation formula we get

0 0

0 px −py

0 0

= (1 − cos φ)pz 0 + cos φ py − sin φ p0x

1 p0z 0

cos φ sin φ 0

= − sin φ cos φ 0 p0

0 0 1

= R01 p0 .

Note that the rotation matrix R01 can be interpreted in two different ways. The

first interpretation is that it transforms the fixed vector p from an expression in

frame F 0 to an expression in frame F 1 where F 1 has been obtained from F 0

by a right-handed rotation. The second interpretation is that it rotates a vector

p though a left-handed rotation to a new vector q in the same reference frame.

Right-handed rotations of vectors are obtained by using (R01 )T .

5

1.2 Quadrotor Coordinate Frames

For quadrotors there are several coordinate systems that are of interest. In this

section we will define and describe the following coordinate frames: the inertial

frame, the vehicle frame, the vehicle-1 frame, the vehicle-2 frame, and the body

frame. Throughout the book we assume a flat, non-rotating earth: a valid assump-

tion for quadrotors.

The inertial coordinate system is an earth fixed coordinate system with origin at

the defined home location. As shown in Figure 4, the unit vector îi is directed

North, ĵ i is directed East, and k̂ i is directed toward the center of the earth.

Figure 4: The inertial coordinate frame. The x-axis points North, the y-axis points

East, and the z-axis points into the Earth.

The origin of the vehicle frame is at the center of mass of the quadrotor. However,

the axes of F v are aligned with the axis of the inertial frame F i . In other words,

the unit vector îv points North, ĵ v points East, and k̂ v points toward the center of

the earth, as shown in Figure 5.

The origin of the vehicle-1 frame is identical to the vehicle frame, i.e, the the

center of gravity. However, F v1 is positively rotated about k̂ v by the yaw angle ψ

so that if the airframe is not rolling or pitching, then îv1 would point out the nose

6

Figure 5: The vehicle coordinate frame. The x-axis points North, the y-axis points

East, 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-axis

points out the nose of the airframe, the y-axis points out the right wing, and the

z-axis points into the Earth.

of the airframe, ĵ v1 points out the right wing, and k̂ v1 is aligned with k̂ v and points

into the earth. The vehicle-1 frame is shown in Figure 6.

The transformation from F v to F v1 is given by

where

cos ψ sin ψ 0

Rvv1 (ψ) = − sin ψ cos ψ 0 .

0 0 1

7

1.2.4 The vehicle-2 frame F v2 .

The origin of the vehicle-2 frame is again the center of gravity and is obtained by

rotating the vehicle-1 frame in a right-handed rotation about the ĵ v1 axis by the

pitch angle θ. If the roll angle is zero, then îv2 points out the nose of the airframe,

ĵ v2 points out the right wing, and k̂ v2 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 out

the nose of the airframe, the y-axis points out the right wing, and the z-axis points

out the belly.

pv2 = Rv1

v2

(θ)pv1 ,

where

cos θ 0 − sin θ

v2

Rv1 (θ) = 0 1 0 .

sin θ 0 cos θ

The body frame is obtained by rotating the vehicle-2 frame in a right handed

rotation about îv2 by the roll angle φ. Therefore, the origin is the center-of-gravity,

îb points out the nose of the airframe, ĵ b points out the right wing, and k̂ b points

out the belly. The body frame is shown in Figure 8.

The transformation from F v2 to F b is given by

pb = Rv2

b

(φ)pv2 ,

8

Figure 8: The body frame. The x-axis points out the nose of the airframe, the

y-axis points out the right wing, and the z-axis points out the belly.

where

1 0 0

b

Rv2 (φ) = 0 cos φ sin φ .

0 − sin φ cos φ

The transformation from the vehicle frame to the body frame is given by

b v2

(φ)Rv1 (θ)Rvv1 (ψ)

1 0 0 cos θ 0 − sin θ cos ψ sin ψ 0

= 0 cos φ sin φ 0 1 0 − sin ψ cos ψ 0

0 − sin φ cos φ sin θ 0 cos θ 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θ

4 4

where cφ = cos φ and sφ = sin φ.

In 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 represent

the body frame of a quadrotor. Suppose that the vector p is moving in F b and that

F b is rotating and translating with respect to F i . Our objective is to find the time

derivative of p as seen from frame F i .

We will derive the appropriate equation through two steps. Assume first that

F is not rotating with respect to F i . Denoting the time derivative of p in frame

b

9

Figure 9: Derivation of the equation of Coriolis.

d

F i as dti

p we get

d d

p= p. (4)

dti dtb

On the other hand, assume that p is fixed in F b but that F b is rotating with respect

to F i , and let ŝ be the instantaneous axis of rotation and δφ the (right-handed)

rotation angle. Then the rotation formula (3) gives

δp δφ

≈ ŝ × p.

δt δt

Taking the limit as δt → 0 and defining the angular velocity of F b with respect to

4

F i as ωb/i = ŝφ̇ we get

d

p = ωb/i × p. (5)

dti

Since differentiation is a linear operator we can combine Equations (4) and (5) to

obtain

d d

p= p + ωb/i × p, (6)

dti dtb

which is the equation of Coriolis.

10

2 Kinematics and Dynamics

In this chapter we derive the expressions for the kinematics and the dynamics of a

rigid body. While the expressions derived in this chapter are general to any rigid

body, we will use notation and coordinate frames that are typical in the aeronautics

literature. In particular, in Section 2.1 we define the notation that will be used for

the state variables of a quadrotor. In Section 2.2 we derive the expressions for the

kinematics, and in Section 2.3 we derive the dynamics.

The state variables of the quadrotor are the following twelve quantities

pe = the inertial (east) position of the quadrotor along ĵ i in F i ,

h = the altitude of the aircraft measured along −k̂ i in F i ,

u = the body frame velocity measured along îb in F b ,

v = the body frame velocity measured along ĵ b in F b ,

w= the body frame velocity measured along k̂ b in F b ,

φ= the roll angle defined with respect to F v2 ,

θ= the pitch angle defined with respect to F v1 ,

ψ= the yaw angle defined with respect to F v ,

p = the roll rate measured along îb in F b ,

q = the pitch rate measured along ĵ b in F b ,

r = the yaw rate measured along k̂ b 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 the

negative 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 Euler

angles (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

The 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 between

position and velocities is given by

pn u

d v

p e = Rb v

dt

−h w

u

b T

= (Rv ) v

w

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

= cθsψ sφsθsψ + cφcψ cφsθsψ − sφcψ v .

−sθ sφcθ cφcθ w

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 different

coordinate frames. The angular rates are defined in the body frame F b , whereas

the roll angle φ is defined in F v2 , the pitch angle θ is defined in Fv1 , and the yaw

angle ψ is defined in the vehicle frame F v .

We need to relate p, q, and r to φ̇, θ̇, and ψ̇. Since φ̇, θ̇, ψ̇ are small and noting

that

b v2

Rv2 (φ̇) = Rv1 (θ̇) = Rvv1 (ψ̇) = I,

12

we get

p φ̇ 0 0

q = Rv2

b

(φ̇) 0 + Rv2

b

(φ)Rv1v2

(θ̇) θ̇ + Rv2

b v2

(φ)Rv1 (θ)Rv→v1 (ψ̇) 0

r 0 0 ψ̇

φ̇ 1 0 0 0 1 0 0 cos θ 0 − sin θ 0

= 0 + 0 cos φ sin φ θ̇ + 0 cos φ sin φ 0 1 0 0

0 0 − sin φ cos φ 0 0 − sin φ cos φ sin θ 0 cos θ ψ̇

1 0 −sθ φ̇

= 0 cφ sφcθ θ̇ . (7)

0 −sφ cφcθ ψ̇

Inverting we get

φ̇ 1 sin(φ) tan(θ) cos(φ) tan(θ) p

θ̇ = 0 cos(φ) − sin(φ) q . (8)

ψ̇ 0 sin(φ) sec(θ) cos(φ) sec(θ) r

Let v be the velocity vector of the quadrotor. Newton’s laws only hold in inertial

frames, therefore Newton’s law applied to the translational motion is

dv

m = f,

dti

where m is the mass of the quadrotor, f is the total applied to the quadrotor, and

d

dti

is the time derivative in the inertial frame. From the equation of Coriolis we

have µ ¶

dv dv

m =m + ω b/i × v = f , (9)

dti dtb

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 body

4 4

coordinates, where vb = (u, v, w)T , and ωb/i b

= (p, q, r)T . Therefore, in body

coordinates, Eq. (9) becomes

u̇ rv − qw fx

v̇ = pw − ru + 1 fy , (10)

m

ẇ qu − pv fz

13

4

where f b = (fx , fy , fz )T .

For rotational motion, Newton’s second law states that

dhb

= m,

dti

where h is the angular momentum and m is the applied torque. Using the equation

of Coriolis we have

dh dh

= + ω b/i × h = m. (11)

dti dtb

Again, Eq. (11) is most easily resolved in body coordinates where hb = Jωb/i

b

R 2 R R

(y R+ z 2 ) dm R − xy dm − R xz dm

J = − R xy dm (x2R+ z 2 ) dm R − yz dm

− xz dm − yz dm (x2 + y 2 ) dm

Jx −Jxy −Jxz

4

= −Jxy Jy −Jyz .

−Jxz −Jyz Jz

Figure 11: The moments of inertia for the quadrotor are calculated assuming a

spherical dense center with mass M and radius R, and point masses of mass m

located at a distance of ` from the center.

As shown in Figure 11, the quadrotor is essentially symmetric about all three

axes, therefore Jxy = Jxz = Jyz = 0 which implies that

Jx 0 0

J = 0 Jy 0 .

0 0 Jz

14

Therefore

1

Jx

0 0

J−1 = 0 1

Jy

0 .

1

0 0 Jz

2M R2

Jx = + 2`2 m

5

2M R2

Jy = + 2`2 m

5

2M R2

Jz = + 4`2 m.

5

4

Defining mb = (τφ , τθ , τψ )T we can write Eq. (11) in body coordinates as

1

ṗ Jx

0 0 0 r −q Jx 0 0 p τφ

q̇ = 0 J1 0 −r 0 p 0 Jy 0 q + τθ

y

ṙ 0 0 J1z q −p 0 0 0 Jz r τψ

J −J

1

y

Jx

z

qr τ

Jx φ

pr

+ Jy τθ .

1

= JzJ−J

y

x

Jx −Jy 1

Jz

pq τ

Jz ψ

The six degree of freedom model for the quadrotor kinematics and dynamics

can be summarized as follows:

ṗn cθcψ sφsθcψ − cφsψ cφsθcψ + sφsψ u

ṗe = cθsψ sφsθsψ + cφcψ cφsθsψ − sφcψ v (12)

ḣ sθ −sφcθ −cφcθ w

u̇ rv − qw fx

v̇ = pw − ru + 1 fy , (13)

m

ẇ qu − pv fz

φ̇ 1 sin φ tan θ cos φ tan θ p

θ̇ = 0 cos φ − sin φ q (14)

sin φ cos φ

ψ̇ 0 cos θ cos θ

r

Jy −Jz 1

ṗ Jx

qr τ

Jx φ

q̇ = Jz −Jx

Jy pr + Jy τθ .

1

(15)

ṙ J −J 1

x

J

y

pq τ

Jz ψ

z

15

3 Forces and Moments

The objective of this section is to describe the forces and torques that act on the

quadrotor. Since there are no aerodynamic lifting surfaces, we will assume that

the aerodynamic forces and moments are negligible.

The forces and moments are primarily due to gravity and the four propellers.

front

left right

back

Figure 12: The top view of the quadrotor. Each motor produces an upward force

F and a torque τ . The front and back motors spin clockwise and the right and left

motors 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 by

F = 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 back

motors as

τθ = `(Ff − Fb ).

Due to Newton’s third law, the drag of the propellers produces a yawing torque

on the body of the quadrotor. The direction of the torque will be in the oppositive

direction of the motion of the propeller. Therefore the total yawing torque is given

by

τψ = τr + τl − τf − τb .

The lift and drag produced by the propellers is proportional to the square of the

angular velocity. We will assume that the angular velocity is directly proportional

to the pulse width modulation commend sent to the motor. Therefore, the force

and torque of each motor can be expressed as

F∗ = k1 δ∗

τ∗ = k2 δ∗ ,

motor command signal, and ∗ represents f , r, b, and l.

Therefore, the forces and torques on the quadrotor can be written in matrix

form as

F k1 k1 k1 k1 δf δf

τφ 0 −`k1 0 `k1 δr 4 δr

= = M .

τθ `k1 0 `k1 0 δb δb

τψ −k2 k2 −k2 k2 δl δl

The control strategies derived in subsequent sections will specify forces and

torques. The actual motors commands can be found as

δf F

δr

= M−1 τφ .

δb τθ

δl τψ

17

Note that the pulse width modulation commands are required to be between zero

and one.

In addition to the force exerted by the motor, gravity also exerts a force on the

quadrotor. In the vehicle frame F v , the gravity force acting on the center of mass

is given by

0

fgv = 0 .

mg

However, since v in Equation (13) is expressed in F b , we must transform to the

body frame to give

0

b

b

fg = Rv 0

mg

−mg sin θ

= mg cos θ sin φ .

mg cos θ cos φ

Therefore, equations (12)–(15) become

ṗn cθcψ sφsθcψ − cφsψ cφsθcψ + sφsψ u

ṗe = cθsψ sφsθsψ + cφcψ cφsθsψ − sφcψ v (16)

ḣ sθ −sφcθ −cφcθ w

u̇ rv − qw −g sin θ 0

v̇ = pw − ru + g cos θ sin φ + 1 0 , (17)

m

ẇ qu − pv g cos θ cos φ −F

φ̇ 1 sin φ tan θ cos φ tan θ p

θ̇ = 0 cos φ − sin φ q , (18)

sin φ cos φ

ψ̇ 0 cos θ cos θ

r

Jy −Jz 1

ṗ Jx

qr τ

Jx φ

q̇ = Jz −Jx

Jy pr + Jy τθ .

1

(19)

ṙ Jx −Jy 1

J

pq τ

Jz ψ

z

4 Simplified Models

Equations (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 significant

insight into the motion. The second reason is that the position and orientation

are relative to the inertial world fixed frame, whereas camera measurements will

measure position and orientation of the target with respect to the camera frame.

For the quadrotor, we are not able to estimate the inertial position or the heading

angle ψ. Rather, we will be interested in the relative position and heading of the

quadrotor with respect to a ground target. The relative position of the quadrotor

will be measured in the vehicle-1 frame, i.e., the vehicle frame after it has been

rotated 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 vehicle

centered quantities as opposed to inertial quantities. Let px , py , and pz denote

the relative position vector between the target and the vehicle resolved in the v1

frame. Therefore Eq (16) becomes

ṗx cθ sφsθ cφsθ u

ṗy = 0 cφ −sφ v. (20)

ṗz −sθ sφcθ cφcθ w

Assuming that φ and θ are small, Equation (18) can be simplified as

φ̇ p

θ̇ = q . (21)

ψ̇ r

Similarly, Equation (19) is simplified by assuming that the Coriolis terms qr, pr,

and pq, are small to obtain

1

ṗ Jx φ

τ

q̇ = J τθ .

1

(22)

y

ṙ 1

Jz ψ

τ

Combining Eq. (21) and (22) we get

1

φ̈ Jx φ

τ

θ̈ = J1 τθ . (23)

y

1

ψ̈ Jz ψ

τ

19

Differentiating Eq. (16) and neglecting Ṙbv gives

p̈n cθcψ sφsθcψ − cφsψ cφsθcψ + sφsψ u̇

p̈e = cθsψ sφsθsψ + cφcψ cφsθsψ − sφcψ v̇ . (24)

p̈d −sθ sφcθ cφcθ ẇ

Neglecting the Coriolis terms and plugging Eq. (17) into Eq. (24) and simplifying

gives

p̈n 0 −cφsθcψ − sφsψ

p̈e = 0 + −cφsθsψ + sφcψ F . (25)

m

p̈d g −cφcθ

Therefore, the simplified inertial model is given by

F

p̈n = (− cos φ sin θ cos ψ − sin φ sin ψ) (26)

m

F

p̈e = (− cos φ sin θ sin ψ + sin φ cos ψ) (27)

m

F

p̈d = g − (cos φ cos θ) (28)

m

1

φ̈ = τφ (29)

Jx

1

θ̈ = τθ (30)

Jy

1

ψ̈ = τψ . (31)

Jz

(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. In

this setting heading is irrelevant. Therefore, instead of expressing the translational

dynamics in the inertial frame, we will express them in the vehicle-1 frame F v1 ,

which is equivalent to the inertial frame after rotating by the heading angle.

Differentiating Eq. (20) and neglecting Ṙbv1 gives

p̈x cθ sφsθ cφsθ u̇

p̈y = 0 cφ −sφ v̇ . (33)

p̈z −sθ sφcθ cφcθ ẇ

20

Neglecting the Coriolis terms and plugging Eq. (17) into Eq. (33) and simplifying

gives

p̈x 0 −cφsθ

p̈y = 0 + sφ F . (34)

m

p̈z g −cφcθ

Therefore, the simplified model in the vehicle-1 frame is given by

F

p̈x = − cos φ sin θ (35)

m

F

p̈y = sin φ (36)

m

F

p̈z = g − cos φ cos θ (37)

m

1

φ̈ = τφ (38)

Jx

1

θ̈ = τθ (39)

Jy

1

ψ̈ = τψ . (40)

Jz

(41)

5 Sensors

5.1 Rate Gyros

A MEMS rate gyro contains a small vibrating lever. When the lever undergoes

an angular rotation, Coriolis effects change the frequency of the vibration, thus

detecting the rotation. A brief description of the physics of rate gyros can be

found at RateSensorAppNote.pdf.

The output of the rate gyro is given by

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 be

given on the spec sheet of the sensor. However, due to variations in manufacturing

it is imprecisely known. The bias term βgyro is strongly dependent on temperature

and should be calibrated prior to each flight.

21

If three rate gyros are aligned along the x, y, and z axes of the quadrotor, then

the rate gyros measure the angular body rates p, q, and r as follows:

ygyro,x = kgyro,x p + βgyro,x + ηgyro,x

ygyro,y = kgyro,y q + βgyro,y + ηgyro,y

ygyro,z = kgyro,z r + β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 samples

the gyros at approximately 120 Hz.

5.2 Accelerometers

A MEMS accelerometer contains a small plate attached to torsion levers. The

plate rotates under acceleration and changes the capacitance between the plate

and the surrounding walls [3].

The output of the accelerometers is given by

yacc = kacc A + β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 given

on the spec sheet of the sensor. However, due to variations in manufacturing it is

imprecisely known. The bias term βacc is strongly dependent on temperature and

should be calibrated prior to each flight.

Accelerometers measure the specific force in the body frame of the vehicle. A

physically intuitive explanation is given in [1, p. 13-15]. Additional explanation

is given in [4, p. 27]. Mathematically we have

ax

ay = 1 (F − Fgravity )

m

az

1

= v̇ + ω bb/i × v − Fgravity .

m

In component form we have

ax = u̇ + qw − rv + g sin θ

ay = v̇ + ru − pw − g cos θ sin φ

az = ẇ + 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 note

that 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 the

accelerometers are used to measure the gravity vector and thereby extract roll and

pitch angles.

MEMS accelerometers are analog devices that are sampled by the on-board

processer. We will assume that the sample rate is given by Ts . The Kestrel autopi-

lot samples the accelerometers at approximately 120 Hz.

5.3 Camera

The control objective is to hold the position of the quadrotor over a ground based

target 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 the

camera is aligned with the body frame z-axis and so that the x-axis of the camera

points out the right of the quadrotor and the y-axis of the camera points to the

back of the quadrotor.

The camera model is shown in Figure 14. The position of the target in the

vehicle-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 in

Figure 15, we can see that

µ ¶

η

py = pz tan φ − ²x , (42)

My

where η is the camera field-of-view, and My is the number of pixels along the

camera y-axis. In Figure 15, both py and ²x are negative. Positive values are

toward the right rotor. A similar equation can be derived for px as

µ ¶

η

px = −pz tan θ − ²y . (43)

My

23

Figure 14: Camera model for the quadrotor.

6 State Estimation

The objective of this section is to describe techniques for estimating the state of the

quadrotor 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 rate

gyros. The remain states require a Kalman filter. Both are discussed below.

The Laplace transforms representation of a simple low-pass filter is given by

a

Y (s) = U (s),

s+a

24

Optical

Axis

Target

Figure 15: The geometry introduced by the vision system. The height above

ground is given by −pz , the lateral position error is py , the roll angle is φ, the

field-of-view of the camera is η, the lateral pixel location of the target in the image

is ²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

ẏ = −ay + au. (44)

Using a zeroth order approximation of the derivative we get

y(t + T ) − y(t)

= −ay(t) + au(t),

T

where T is the sample rate. Solving for y(t + T ) we get

y(t + T ) = (1 − aT )y(t) + aT u(t).

For the zeroth order approximation to be valid we need aT ¿ 1. If we let α = aT

then 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 is

relatively noise free, then α should be close to unity.

In the derivation of the discrete-time implementation of the low-pass filter, it is

possible to be more precise. In particular, returning to Equation (44), from linear

systems theory, it is well known that the solution is given by

Z T

−aT

y(t + T ) = e y(t) + a e−a(T −τ ) u(τ ) dτ.

0

Assuming that u(t) is constant between sample periods results in the expression

Z T

−aT

y(t + T ) = e y(t) + a e−a(T −τ ) dτ u(t)y(t + T )

0

= e−aT y(t) + (1 − e−aT )u(t). (45)

2

Note that since ex = 1 + x + x2! + . . . 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 preferable

way to implement a low-pass filter in digital hardware.

We will use the notation LP F (·) to represent the low-pass filter operator.

Therefore x̂ = LP F (x) is the low-pass filtered version of x.

The angular rates p, q, and r can be estimated by low-pass filtering the rate gyro

signals:

p̂ = LP F (ygyro,x ) (46)

q̂ = LP F (ygyro,y ) (47)

r̂ = LP F (ygyro,z ). (48)

The objective of this section is to briefly review observer theory.

26

Suppose that we have a linear time-invariant system modeled by the equations

ẋ = Ax + Bu

y = Cx.

A continuous-time observer for this system is given by the equation

x̂˙ = |Ax̂ {z

+ Bu} + L (y − C x̂), (49)

| {z }

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 complex

plane.

In practice, the sensors are usually sampled and processed in digital hardware

at 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 using

the equation

x̂˙ = Ax̂ + Bu (50)

and then to update the estimate when a measurement is received using the equation

x̂+ = x̂− + L(y(tk ) − C x̂− ),

where tk is the instant in time that the measurement is received and x̂− is the

state estimate produced by Equation (50) at time tk . Equation (50) is then re-

instantiated with initial conditions given by x̂+ . The continuous-discrete observer

is summarized in Table 6.3.

The observation process is shown graphically in Figure 16. Note that it is not

necessary to have a fixed sample rate. The continuous-discrete observer can be

implemented using Algorithm 1.

Note that we did not use the fact that the process was linear. Suppose instead

that we have a nonlinear system of the form

ẋ = 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

1: 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.}

Tout

5: x̂ = x̂ + N (Ax̂ + Bu)

6: end for

7: if A measurement has been received from sensor i then {Correction: Mea-

surement Update}

8: x̂ = x̂ + Li (yi − Ci x̂)

9: end if

28

System model:

ẋ = Ax + Bu

y(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 ) − C x̂− (tk )) .

Let X = (x1 , . . . , xn )T be a random vector whose elements are random variables.

The mean, or expected value of X is denoted by

µ1 E{x1 }

µ = ... = ... = E{X},

µn E{xn }

where Z

E{xi } = ξfi (ξ) dξ,

and f (·) is the probability density function for xi . Given any pair of components

xi 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:

p

stdev(xi ) = σi = Σii .

29

System model:

ẋ = 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 ))) .

The covariances associated with a random vector X can be grouped into a matrix

known as the covariance matrix:

Σ11 Σ12 · · · Σ1n

Σ21 Σ22 · · · Σ2n

T T T

Σ = .. . . .. = E{(X − µ)(X − µ) } = E{XX } − µµ .

. . .

Σn1 Σn2 · · · Σnn

implies that its eigenvalues are real and nonnegative.

The probability density function for a Gaussian random variable is given by

(x−µx )2

1 −

fx (x) = √ e σx2 ,

2πσx

where µx is the mean of x and σx is the standard deviation. The vector equivalent

is given by

· ¸

1 1 T −1

fX (X) = √ exp − (X − µ) Σ (X − µ) ,

2π det Σ 2

in which case we write

X ∼ N (µ, Σ) ,

30

Figure 17: Level curves for the pdf of a 2D Gaussian random variable.

Figure 17 shows the level curves for a 2D Gaussian random variable with

different covariance matrices.

In this section we assume the following state model:

ẋ = Ax + Bu + Gξ

yk = Cxk + ηk ,

the measurement noise at time tk , ξ is a zero-mean Gaussian random process with

covariance Q, and ηk is a zero-mean Gaussian random variable with covariance

R. 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 ) − C x̂− (tk )) .

Our objective is to pick L to minimize tr(P (t)).

Differentiating x̃ we get

x̃˙ = ẋ − x̂˙

= Ax + Bu + Gξ − Ax̂ − Bu

= Ax̃ + Gξ.

Z t

At

x̃(t) = e x̃0 + eA(t−τ ) Gξ(τ ) dτ.

0

d

Ṗ = E{x̃x̃T }

dt

˙ T + x̃x̃˙ T }

= E{x̃x̃

© ª

= E Ax̃x̃T + Gξ x̃T + x̃x̃T AT + x̃ξ T GT

= AP + P AT + GE{ξ x̃T }T + E{x̃ξ T }GT .

½ Z t ¾

T AT t T T AT (t−τ )

E{ξ x̃ } = E ξ(t)x̃0 e + ξ(t)ξ (τ )G e dτ

0

1

= QGT ,

2

which implies that

Ṗ = AP + P AT + GQGT .

32

6.5.2 At Measurements.

At a measurement we have that

x̃+ = x − x̂+

¡ ¢

= x − x̂− − L Cx + η − C x̂−

= x̃− − LC x̃− − Lη.

Therefore

P + = E{x̃+ x̃+T }

n¡ ¢¡ ¢T o

= E x̃− − LC x̃− − Lη x̃− − LC x̃− − Lη

©

= E x̃− x̃−T − x̃− x̃−T C T LT − x̃− η T LT

− LC x̃− x̃−T + LC x̃− x̃−T C T LT + LC x̃− η T LT

ª

= −Lηx̃−T + Lηx̃−T C T LT + Lηη T LT

= P − − P − C T LT − LCP − + LCP − C T LT + LRLT . (53)

Our objective is to pick L to minimize tr(P + ). A necessary condition is

∂

tr(P + ) = −P − C T − P − C T + 2LCP − C T + 2LR = 0

∂L

=⇒ 2L(R + CP − C T ) = 2P − C T

=⇒ L = P − C T (R + CP − C T )−1 .

Plugging back into Equation (53) give

P + = P − + P − C T (R + CP − C T )−1 CP − − P − C T (R + CP − C T )−1 CP −

+ P − C T (R + CP − C T )−1 (CP − C T + R)(R + CP − C T )−1 CP −

= P − − P − C T (R + CP − C T )−1 CP −

= (I − P − C T (R + CP − C T )−1 C)P −

= (I − LC)P − .

For linear systems, the continuous-discrete Kalman filter is summarized in

Table 6.5.2

If the system is nonlinear, then the Kalman filter can still be applied but we

need to linearize the nonlinear equations in order to compute the error covariance

matrix P and the Kalman gain L. The extended Kalman filter (EKF) is given in

Table 6.5.2, and an algorithm to implement the EKF is given in Algorithm 2.

33

System model:

ẋ = Ax + Bu + ξ

yi (tk ) = Ci x(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 Ṗ = AP + P AT + Q.

Li = P − CiT (Ri + Ci P CiT )−1 ,

P + = (I − Li Ci )P − ,

x̂+ (tk ) = x̂− (tk ) + L (y(tk ) − C x̂− (tk )) .

In this section we will discuss the application of Algorithm 2 to the quadrotor.

We would like to estimate the state

p̂x

p̂y

p̂z

p̂˙x

x̂ = p̂˙y ,

˙

p̂z

φ̂

θ̂

ψ̂

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:

ẋ = 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),

∂f ¯

Compute A = ∂x x=x̂(t) ,

Propagate Ṗ = AP + P AT + Q.

th

Correction: At ¯ the i sensor measurement (t = tk ):

∂ci ¯

Ci = ∂x x=x̂−

Li = P − CiT (Ri + Ci P CiT )−1 ,

P + = (I − Li Ci )P − ,

x̂+ (tk ) = x̂− (tk ) + L (y(tk ) − ci (x̂− (tk ))) .

ṗx

ṗy

ṗz

cos φ sin θa

z

f (x, u) =

−sinφa z

,

g + cos φ cos θaz

p + q sin φ tan θ + r cos φ tan θ

q cos φ − r sin φ

sin φ

q 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 Filter

1: 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.}

Tout

5: x̂ = x̂ + N (f (x̂, u))

6: A = ∂f∂x ¡ out ¢ ¡ ¢

7: P = P + TN AP + P AT + GQGT

8: end for

9: if A measurement has been received from sensor i then {Correction: Mea-

surement Update}

10: Ci = ∂c∂x

i

12: P = (I − Li Ci )P

13: x̂ = x̂ + Li (yi − ci (x̂)).

14: end if

0 0 0 1 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 −sφ sθ az cφ cθ az 0

A= 0 0 0 0 0 0 −c a

φ z 0 0

0 0 0 0 0 0 −sφ cθ az −cφ sθ az 0

0 0 0 0 0 0 qcφtθ − rsφtθ (qsφ + rcφ)/c2 θ 0

0 0 0 0 0 0 −qsφ − rcφ 0 0

qcφ−rsφ tθ

0 0 0 0 0 0 cθ

−(qsφ + rcφ) cθ 0

Note that it may be adequate (not sure) to use a small angle approximation in

36

the model resulting in

ṗx

ṗy

ṗz

θaz

f (x, u) =

−φa z

,

g + az

p

q

r

and

0 0 0 1 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 az 0

A=

0 0 0 0 0 0 −az 0 0.

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 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 take

into account the motion of the target. A feedforward term can be added to f to

account for the target motion as

ṗx − ṁx

ṗy − ṁy

ṗ

z

θaz − m̈x

f (x, u) =

−φa z − m̈ y

,

g + az

p

q

r

where (ṁx , ṁy ) is the velocity of the target in the vehicle-1 frame, and (m̈x , m̈y )

is the acceleration of the target in the vehicle-1 frame.

37

Let the relative position between the quadrotor and the target be denoted by

p = (px , py , pz )T . We can tranform to the camera frame as

v1

b

pv1 ,

where

0 1 0

Rgc = 0 0 1

1 0 0

is the rotation matrix from gimbal coordinates to camera coordinates,

0 0 1

Rbg = 0 1 0

−1 0 0

transforms body coordinates to gimbal coordinates, and

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

b

Rv1 = 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

φθpx + py + φpz

pc = −px + θpz .

θpx − φpy + pz

The model for the pixel coordinates are therefore computed as

4 φθpx + py + φpz

²x = c²x (x) = f

θpx − φpy + pz

4 −px + θpz

²2 = c²y (x) = f .

θpx − φpy + pz

We therefore have the following sensors available to correct the state estimate:

4

sensor 1: altimeter y1 = calt (x) = −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)

4

sensor 4: vision heading y4 = c²ψ (x) = π/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 can

easily be computed numerically using the approximation

.

∂f (x1 , · · · , xi , . . , xn ) f (x1 , · · · , xi + ∆, · · · , xn ) − f (x1 , · · · , xi , · · · , xn )

≈ ,

∂xi ∆

where ∆ is a small constant.

7 Control Design

The control design will be derived directly from Equations (35)–(40). Equa-

tions (38)–(40) are already linear. To simplify Equations (35)–(37) define

4 F

ux = −cosφ sin θ (60)

m

4 F

uy = sin φ (61)

m

4 F

uz = g − cos φ cos θ , (62)

m

to obtain

p̈x = ux (63)

p̈y = uy (64)

p̈z = uz . (65)

The control design proceeds by developing PID control strategies for ux , uy , and

uz . After ux , uy , and uz have been computed, we can compute the desired force

F , and the commanded roll angle φc and the commanded pitch angle θc from

Equations (60)–(62) as follows. From Equation (62) solve for F/m as

F g − uz

= . (66)

m cos φ cos θ

39

Substituting (66) into (61) gives

g − uz

uy = tan φ.

cos θ

Solving for φ and letting this be the commanded roll angle gives

µ ¶

c −1 uy cos θ

φ = tan . (67)

g − uz

Similarly, we can solve for the commanded pitch angle as

µ ¶

c −1 ux

θ = tan . (68)

uz − g

For the altitude hold we need to develop an expression for uz to drive pz to a

desired altitude based on the size of the object in the image. We will assume that

the camera returns the size of the object in the image plane in pixels, which is

denoted 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 the

image plane is denoted by ²s in pixels. The focal length is f , and the height above

the target is −pz .

−pz f

= .

S ²s

40

Solving for pz an differentiating we obtain

f S ²̇s

ṗz = . (69)

²2s

Differentiating again gives

f S²̈s ²̇2s

p̈z = 2 − 2f S 3 .

²s ²s

Substituting uz = p̈z and solving for ²̈s gives

µ 2¶

²s ²̇2

²̈s = uz + 2 s .

fS ²s

Defining µ ¶

4 ²2s ²̇2s

us = uz + 2 , (70)

fS ²s

we get

²̈s = us .

We can now derive a PID controller to drive ²s → ²ds as

Z t

d

us = kps (²s − ²s ) − kds ²̇s + kis (²ds − ²s ) dτ.

0

µ ¶ Z

fS d fS f S ²̇s fS t d

uz = 2 kps (²s − ²s ) − kds 2 + 2 3 ²̇s + kis 2 (² − ²s ) dτ. (71)

²s ²s ²s ²s 0 s

The downside to this equation is that it requires knowledge of the target size S and

the focal length f . This requirement can be removed by incorporating f S into the

PID gains by defining

4

k̂ps = f Skps

4

k̂is = f Skis

4

k̂ds = f Skds ,

and by noting from Equation (69) that f S²2²̇s = w. Therefore Equation (71) be-

s

comes

Ã ! Z

(²ds − ²s ) k̂ds w k̂is t d

uz = k̂ps − +2 ²̇s + 2 (² − ²s ) dτ. (72)

²2s ²2s ²s ²s 0 s

41

7.2 Roll Attitude Hold

The equation of motion for roll is given by Eq. (38) as φ̈ = τp /Jx . We will use a

PID controller to regulate the roll attitude as

Z t

c

τp = kpφ (φ − φ) − kdφ p + kiφ (φc − φ) dτ.

0

The gains kpφ , kiφ , and kdφ are selected one at a time using a method called

successive loop closure. To pick kpφ note that if the input command φc is a step of

value 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

Jx

s

1 + kdφ kpφ

= 0.

s2 + Jx

The characteristic equation including kiφ in Evan’s form verses kiφ is given by

1

Jx

1 + kiφ kdφ kpφ

= 0.

s3 + Jx

s2 + Jx

s

42

The integral gain kiφ can be selected so that damping ratio is not significantly

changed.

The equation of motion for pitch is given by Eq. (39) as θ̈ = τq /Jy . Similar to the

roll attitude hold, we will use a PID controller to regulate pitch as

Z t

c

τq = kpθ (θ − θ) − kdθ q + kiθ (θc − θ) dτ.

0

From Equation 64 the lateral dynamics are given by p̈y = uy , where py is the

relative lateral position which we drive to zero using the PID strategy

Z t

uy = −kpy py − kdy v + kiy py dτ.

0

From Equation 63 the longitudinal dynamics are given by p̈x = ux , where px

is the relative lateral position which we drive to zero using the PID strategy

Z t

ux = −kpx px − kdx u + kix px dτ.

0

The heading dynamics are given in Equation (40) as ψ̈ = τr /Jz . Define ψ d as the

4

inertial heading of the target, and define ψ̃ = ψ − ψ d as the relative heading. The

camera directly measures ψ̃. Assuming that ψ d is constant we get ψ̃¨ = τr /Jz . We

regulate the relative heading with the PID strategy

Z t

τr = −kpψ ψ − kdψ r − kiψ ψ dτ.

0

43

7.6 Feedforward

When the quadrotor is tracking a ground robot, the motion of the robot will cause

tracking 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 to

use 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

ÿ = u,

then the standard PD loop is shown in Figure 20.

Let e = r − y, then

ë = r̈ − ÿ

= r̈ − u

= r̈ − kp e + kd ẏ

= r̈ − kp e − kd ė + kd ṙ.

ë + kd ė + kp e = r̈ + kd ṙ.

1

Therefore, the signal r̈ + kd ṙ drives the error transfer function s2 +kd s+kp

and if w

is not zero, the tracking error will not go to zero.

44

From the expression ë = r̈ − u we see that if instead of u = kp e − kd ẏ, we

instead use

u = r̈ + kp e + kd ė,

then we get

ë + kd ė + kp e = 0,

which ensures that e(t) → 0 independent of r(t). The associated block diagram

is shown in Figure 21.

Now consider the lateral motion of the quadrotor in the cage where

ÿ = g tan φ,

where y is the position of the quadrotor, g is the gravity constant, and φ is the roll

4

angle. Let m(t) be the position of the target and define y = m − r as the relative

position. Then

ÿ = m̈ − r̈

= m̈ − g tan φ.

If by some means we know the target velocity ṁ and the target acceleration m̈,

then we should pick φ such that

g tan φ = m̈ + kp y + kd ẏ,

or µ ¶

−1 m̈ + kp y + kd ẏ

φ = tan .

g

45

Given the decoupling terms described at the beginning of this section, the

motion model for the quadrotor in the vehicle-1 frame is given by

p̈x = ux

p̈y = uy

ψ̈ = uψ .

Let the position of the target in the vehicle-1 frame be given by (mx , my ) and let

it orientation be given by ψt , and define

p̃x = mx − px

p̃y = my − py

ψ̃ = ψt − ψ.

Differentiating twice we get

p̃¨x = m̈x − ux

p̃¨y = m̈y − uy

ψ̃¨ = ψ̈ − u .

t ψ

ux = m̈x + P IDx

uy = m̈y + P IDy

uψ = ψ̈t + P IDψ

where P ID∗ are the PID controllers derived earlier in this chapter.

The motion model for the target is given by

ṁn = Vt cos ψt

ṁe = Vt sin ψt

V̇t = u1

ψ̇t = u2 ,

where (mn , me ) is the inertial position of the target, Vt is the ground speed of the

target, and u1 and u2 are robot control signals. In the vehicle-1 frame we have

µ ¶ µ ¶µ ¶ µ ¶

ṁx cos ψ − sin ψ Vt cos ψt Vt cos ψ̃

= = ,

ṁy sin ψ cos ψ Vt sin ψt Vt sin ψ̃

46

where the camera measures the relative heading ψ̃ = ψ − ψt . Differentiating we

obtain µ ¶ µ ¶µ ¶

m̈x cos ψ̃ sin ψ̃ u1

= .

m̈y − sin ψ̃ cos ψ̃ u2 − ψ̇

To implement feedforward on the quadrotors, we must communicate with the

robot 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

- Physics First YearDiunggah olehaliraza27298
- 6FX2001-4DA10 Datasheet EnDiunggah olehandrei.ciobanasu
- Gyro submarino_Octans3000.pdfDiunggah olehArnoldo López Méndez
- 10-EASY-ROB-Chapter-08Diunggah olehapi-3714448
- Mission Critical Winter 2012Diunggah olehKanishka Lankatillake
- INS AlgorithmsDiunggah olehversine
- cruise messile workingDiunggah olehapi-19807868
- Sample ProblemsDiunggah olehChris Dackow
- Rakesh IJAMM 1Diunggah olehRakesh Kumar
- Analysis of the Electrolytic Codeposition of Non-Brownian PDiunggah olehmomenzi
- RailwayDiunggah olehRaghul Ramasamy
- Multi-uav Int Nav With VisionDiunggah olehJuan Manuel Mauro
- Balancing Pump ImpellersDiunggah olehIziyon Harrison Osaloboh
- 975956Diunggah olehAhmed Salim
- DH-derivDiunggah olehأحمد دعبس
- MAPSeqns UpdatedDiunggah olehSkoplaki Elisa
- Ijcas SEO 2006Diunggah olehPhạm Ngọc Hòa
- 2009 FerreiraPintoMatosCruz bDiunggah olehganam
- crisanbucklingDiunggah olehalfonxxl
- pg1017eDiunggah olehscrbdgharavi
- GPS World - September 2015Diunggah olehAlex
- MP-043-17 lỗi sensorDiunggah olehtangphamvan
- CAE Mid Term ReportDiunggah olehPratik Halyal
- (Www.entrance-exam.net)-JNTU,B.tech, ME, 1st Sem, Engineering Mechanics Sample Paper 1Diunggah olehsanagavarapu
- Physics: Mechanics Review SheetDiunggah olehCody Swain
- Physics for You - October 2016Diunggah olehRishikeshKumar
- final_exam_formu.pdfDiunggah olehJoØrsh Ênrique Tu Xikytø NînîØflow
- Moment of Inertia.pptDiunggah olehsenthilcae
- PiggingDiunggah olehNikhil Mohan
- 1611.07329.pdfDiunggah olehjornada660

- Path Following Controller for a Quadrotor Helicopter.pdfDiunggah olehflausen
- Hanavan Model FullDiunggah olehEd Jones
- Cold Rolling and Annealing Textures in Low Carbon and Extra Low Carbon SteelsDiunggah olehZhiyong Jia
- CSE169_04Diunggah olehmatkabhai
- Manual Version 4-5Diunggah olehZuhair Mahmud
- Assignment 2.pdfDiunggah olehrodrigocampospk
- Azimuth Elevation PolarizationDiunggah olehtunisianou
- Probabilistic Robotics.pdfDiunggah olehram17yadav
- L3 - Spatial Transformations 2 V1Diunggah olehzul_fadhli_1988
- Submarine Dynamic ModelingDiunggah olehkooec
- geometry3D_techrepDiunggah olehAlejandro Gaston
- CP-013Diunggah olehmoein_joon27
- MAE331Lecture8.pdfDiunggah olehBeatrice Firan
- MTB Tech BlogDiunggah olehJong-hun Lee
- AUV_PIDDiunggah olehLe Anh Tuan
- Oculus SDK Overview (1)Diunggah olehbabichem
- A Motion Cueing Model for Mining and Forestry Simulator Platforms Based on Model Predictive ControlDiunggah olehMiffeltoffel1567
- Lesson 09Diunggah olehSachin Shende
- Calibration of Machine SquarenessDiunggah olehsunoval2013
- Kinematics of RobotsDiunggah olehRakeshSaini
- 287-293Diunggah olehFici Iman N
- 2.2.a.lecture Gianluca AntonelliDiunggah olehЋирка Фејзбуџарка
- Simulation and Control of a Quadrotor Unmanned Aerial VehicleDiunggah olehHoàngCôngAnh
- CS0111Diunggah olehShailendra Rajput
- tesiscontrolydeterminaciondeposicionnanosatellitefinlandiaDiunggah olehJulio David Vilca Pizarro
- Optimally-Robust Nonlinear Control of a Class of AUVsDiunggah olehtorsten_pf
- Modeling and Control of an Unmanned Underwater VehicleDiunggah oleharash501
- Pramod S. Joag-An Introduction to Vectors, Vector Operators and Vector Analysis-Cambridge University Press (2019)Diunggah olehangeladrian200
- Active Guidance and Dynamic Flight Mechanics for Model RocketsDiunggah olehĐăng Khôi Trần
- Automatic Control II Hover V7Diunggah olehArsalan Ahmad