IEEE Robotics & Automation Magazine - September 2010 - 58

2 3 2
p
ch
4q5 ¼ 4 0
r
sh

F2
F3

M2
L

zB yB

M3

xB

F1

F4

M1
M4

mg

zW

r

yW
xW

Figure 2. Coordinate systems and forces/moments acting on a
quadrotor frame.

axes xW , yW , and zW , with zW pointing upward. The body
frame, B, is attached to the center of mass of the quadrotor,
with xB coinciding with the preferred forward direction and
zB perpendicular to the plane of the rotors pointing vertically
up during perfect hover (see Figure 2). Rotor 1 is on the positive xB axis, Rotor 2 on the positive yB axis, Rotor 3 on the
negative xB axis, and Rotor 4 on the negative yB axis. We use
Z À X À Y Euler angles to model the rotation of the quadrotor in the world frame. To get from W to B, we first rotate
about zW by the yaw angle, w, then rotate about the intermediate x axis by the roll angle, /, and finally rotate about the yB
axis by the pitch angle, h. The rotation matrix for transforming
the coordinates from B to W is given by
2

cwch À s/swsh
R ¼ 4 chsw þ cws/sh
Àc/sh

3

Àc/sw
c/cw
s/

cwsh þ chs/sw
swsh À cwchs/ 5,
c/ch

In addition to forces, each rotor produces a moment perpendicular to the plane of rotation of the blade, Mi . Rotors 1 and
3 rotate in the ÀzB direction while Rotors 2 and 4 rotate in
the zB direction. Since the moment produced on the quadrotor is opposite to the direction of rotation of the blades, M1
and M3 act in the zB direction while M2 and M4 act in the
ÀzB direction. We let L be the distance from the axis of rotation of the rotors to the center of the quadrotor. The moment
of inertia matrix referenced to the center of mass along the
xB À yB À zB axes, I, is found by weighing the individual
components of the quadrotor and building a physically accurate model in SolidWorks. The angular acceleration determined by the Euler equations is
2 3 2
3 2 3 2 3
L(F2 À F4 )
p
p
p_
5 À 4 q 5 3 I 4 q 5: (2)
I 4 q_ 5 ¼ 4
L(F3 À F1 )
r
M1 À M2 þ M3 À M4
r
r_
Motor Model
Each rotor has an angular speed xi and produces a vertical
force Fi according to
Fi ¼ kF x2i :

(3)

Experimentation with a fixed rotor at steady state shows that
kF % 6:11 3 10À8 N=(r=min2 ). The rotors also produce a
moment according to

where ch and sh denote cos (h) and sin (h), respectively, similarly for / and w. The position vector of the center of mass in
the world frame is denoted by r. The forces on the system are
gravity in the ÀzW direction and the forces from each of the
rotors, Fi , in the zB direction. The equation governing the
acceleration of the center of mass is
2
3
3
0
0
m€r ¼ 4 0 5 þ R4 0 5:
Àmg
RFi

32 3
0 Àc/sh
/_
1
s/ 54 h_ 5:
0 c/ch
w_

2

(1)

The components of angular velocity of the robot in the body
frame are p, q, and r. These values are related to the derivatives of
the roll, pitch, and yaw angles according to

Mi ¼ kM x2i :

(4)

The constant, kM , is determined to be about 1:5 3 10À9
N Á m=(r=min2 ) by matching the performance of the simulation to the real system.
The results of a system-identification exercise suggest that
the rotor speed is related to the commanded speed by a firstorder differential equation
x_ i ¼ km (wides À wi ):
This motor gain, km , is found to be about 20 sÀ1 by matching
the performance of the simulation to the real system. The
desired angular velocities, xdes
i , are limited to a minimum and
maximum value determined through experimentation to be
approximately 1,200 and 7,800 r=min.

Robot Controllers
ΔωF

r T (t)
r(t )

Position
Control

φ des(t )
θ des(t )
ψ des(t )

··r

Attitude
Control

1

Δωφ
Δωθ
Δωψ
1

Ti, Mi Rigid-Body
Motor
Dynamics
Dynamics

·
·
·
p (t ), q(t ), r (t )

Figure 3. The nested control loops for position and attitude control.
58

IEEE Robotics & Automation Magazine

Each robot is controlled independently
by nested feedback loops, as shown in
Figure 3. The inner attitude control loop
uses onboard accelerometers and gyros
to control the roll, pitch, and yaw angles
and runs at approximately 1 kHz [5],
while the outer position control loop
uses the estimates of position and velocity of the center of mass to control the
SEPTEMBER 2010
Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - September 2010
