Nivel III SENSORES

Flying Robots

1. Rigid Body

2. Modeling of quadrotor
The quadrotor structure including the corresponding angular velocities, torques and forces
created by the four rotors.

Figura 1: Inertial fixed reference frame and body fixed reference frame.

The absolute linear position of the quadrotor is defined in the inertial frame x, y, z axes. The
attitude, the angular position is defined in the inertial frame with three Euler angles. Roll angle φ
determines the rotation around the x-axis. Pitch angle θ determines the rotation of the quadrotor
around the y-axis and yaw angle ψ around the z-axis.

Vector state q contains the linear and angular position vectors

Club de Robótica 1
Nivel III SENSORES

   
x φ  
ξ
ξ = y ,
 η = θ ,
 q= (1)
η
z ψ
The origin of the body frame is in the center of mass of the quadrotor. In the body frame, the
linear velocities are determined by v B and the angular velocities by ω B
   
xb ωx
B B
v = yb , ω = ωy 
  
zb ωz
Using Euler to represent of orientation in the environment, Roll, Pitch and Yaw angles denote
the typical changes of attitude of and quadrotor. The complete rotation matrix for moving from
the inertial frame to the body frame is given by [?]

RIB = R(φ) R(θ) R(ψ) (2)

Performing the multiplication, the complete rotation from the inertial frame to the body frame
is given by

 
cos θ cos ψ cos θ sin ψ − sin θ
RIB = (sin φ sin θ cos ψ − cos φ sin ψ) (sin φ sin θ sin ψ + cos φ cos ψ) sin φ cos θ  (3)
(cos φ sin θ cos ψ + sin φ sin ψ) (cos φ sin θ sin ψ − sin φ cos ψ) cos φ cos θ
−1 T
The rotation matrix RIB is orthogonal thus RIB = RIB which is the rotation matrix from the
body frame to the inertial frame.

 
cos θ cos ψ (sin φ sin θ cos ψ − cos φ sin ψ) (cos φ sin θ cos ψ + sin φ sin ψ)
I
RB =  cos θ sin ψ (sin φ sin θ sin ψ + cos φ cos ψ) (cos φ sin θ sin ψ − sin φ cos ψ) (4)
− sin θ sin φ cos θ cos φ cos θ
The transformation matrix for angular velocities from the inertial frame to the body frame is
Jη and from the body frame to the inertial frame is Wη−1 .
 
1 sin φ tan θ cos φ tan θ
W (φ, θ, ψ) = 0
 cos φ − sin φ  (5)
sin φ cos φ

0
cos θ cos θ
η̇ = Wη−1 ω B (6)

ω B = Wη η̇ (7)
The quadcopter is assumed to have symmetric structure with the four arms aligned with the
body x- and y-axes. Thus, the inertia matrix is diagonal matrix I in which Ixx = Iyy
 
Ixx 0 0
I =  0 Iyy 0  (8)
0 0 Izz
The angular velocity of rotor i, denoted with ωi , creates force fi in the direction of the rotor
axis. The angular velocity and acceleration of the rotor also create torque τMi around the rotor
axis

Club de Robótica 2
Nivel III SENSORES

fi = k ωi2 , τMi = bωi2 + IM ω̇i (9)

in which the lift constant is k, the drag constant is b and the inertia moment of the rotor is IM .
Usually the effect of ω̇i is considered small and thus it is omitted.

The combined forces of rotors create thrust T in the direction of the body z-axis. Torque τB
consists of the torques τφ , τθ and τψ in the direction of the corresponding body frame angles.
X X
T = fi = −k ωi (10)

l k (ω42 − ω22 )
   
τφ
τ B =  τθ  = l kP(ω32 − ω12 ) (11)
4
τψ i=1 TMi

in which l is the distance between the rotor and the center of mass of the quadcopter.

2.1. Newton-Euler equations

The quadcopter is assumed to be rigid body and thus Newton-Euler equations can be used to
describe its dynamics. In the body frame
T
m v̇ B + ω B × (m v B ) = −RB
I
G+T (12)
In the inertial frame the position is represent [?]:

mξ¨ = G + RB
I
T (13)

where G is the gravity vector and m is the quadrotor mass.

In the body frame, the angular acceleration of the inertia I ω˙B , the centripetal forces ω B ×
(Iω B )
I ω̇ B + ω B × (Iω B ) = τ (14)

So,
I − Izz B B   τφ 
yy
ωy ωz
 B   Ixx   Ixx 
ω̇x 
I − I
  
 τ 
 B   zz xx B B  θ
ω̇y  =  ωx ωz  +   (15)
 Iyy I
  yy 

ω̇zB 
I − I
  
 τψ 
xx yy B B
ωx ωy Izz
Izz

Club de Robótica 3
Nivel III SENSORES

Note 1

Other authors using:  B  

ωx p
ωyB  = q 
ωzB r
Also:    
T U1
 τφ  U2 
 = 
 τθ  U3 
τψ U4

The equations of motion are more conveniently formulated in the body-fixed frame because
of the following reasons:

The inertia matrix is time-invariant.

Advantage of body symmetry can be taken to simplify the equations.
Measurements taken on-board are easily converted to body-fixed frame.
Control forces are almost always given in body-fixed frame.

3. Control modeling
The dynamics of the quadrotor is well described in the previous section. However the most
important concepts can be summarized in equations ??, ?? and ??. The first one shows how the
quadrotor accelerates according to the basic movement commands given.
T


 ẍ = (cos φ sin θ cos ψ + sin φ sin ψ)



 m
ξ = ÿ = (cos φ sin θ sin ψ − sin φ cos ψ) T (16)



 m

z̈ = g + (cos φ cos θ)


4. Stability - Lyapunov
Dynamics equation

I ω̇ = −(ω × Iω) + Γ (17)

ω̇ = −I −1 (ω × Iω) + I −1 Γ (18)

To body frame:

ω = T (Φ)Φ̇ (19)
T
ω = R T (Φ) Φ̇ (20)

Derivate ω:
ω̇ = ṘT T (Φ) Φ̇ + RT Ṫ (Φ)Φ̇ + RT T (Φ) Φ̈ (21)

Club de Robótica 4
Nivel III SENSORES

From ?? replace ω from ?? and ??:

I(ṘT T (Φ) Φ̇ + RT Ṫ (Φ)Φ̇ + RT T (Φ) Φ̈) = −(ω × IRT T (Φ) Φ̇) + Γ (22)

I ṘT T (Φ) Φ̇ + I RT Ṫ (Φ)Φ̇ + I RT T (Φ) Φ̈ = −(ω × IRT T (Φ) Φ̇) + Γ

Club de Robótica 5