You are on page 1of 28

Chapter 3

Quadrotor / Quadcopter
The First Manned Quadrotor
• Quadrotor is a kind of unmanned aerial vehicle
(UAV)
• 29/9/1907: Louis Bréguet & Jacques Bréguet,
under the guidance of Professor Charles
Richet, demonstrated the first flying quadrotor
named Bréguet-Richet Gyroplane No. 1
Advantages of quadrotor
• Quadrotor is a rotary wing UAV
• Its advantages over fixed wing UAVs:
– Vertical Take Off and Landing (VTOL)
– Able to hover
– Able to make slow precise movements.
– Four rotors provide a higher payload capacity
– More flexible in maneuverability through an
environment with many obstacles, or landing in
small areas.
Quadcopter anatomy
Quadrotor structure
• Front motor (Mf) (+x) • Each spinning motor
• Back motor (Mb) (-x) provides
• Right motor (Mr) (+y) – thrust force (T) for lifting
• Left motor (Ml) (-y) – torque () for rotating
• Mf and Mb rotates CW
• Mr and Ml rotates CCW
• This arrangement can
overcome torque effect to
prevent on the spot
spinning of the structure
Basic movements

X (North)

Y (East)

Z (Down)
Reference Frames
• There are a few reference frames to model
the kinematics and dynamics of a quadrotor
– Inertia Frame (Global frame), Fi
– Vehicle Frame, Fv
– Vehicle-1 Frame, Fv1
– Vehicle-2 Frame, Fv2
– Body Frame (Local frame), Fb
X (North)

The inertia frame Y (East)

Z (Down)

• For the context of quadrotor, the Earth is a


flat surface
• The starting position of the quadrotor is the
origin of the global frame or the inertia frame
(Fi)
• Fi: x-y-z axis is right hand system with x
pointing to North, y pointing to East and z
pointing to Down, it is also known as the NED
system
The Vehicle Frame
• Fv is the vehicle frame
• It is the inertia frame, Fi, linear shifted to the
centre of gravity (COG) for the quadrotor
• The coordinates of the COG for the quadrotor
wrt Fi is (xc, yc, zc).
Xv
Fv

Yv

Zv

Xi (North)

Fi

Yi (East)

Zi (Down)
The Vehicle-1 Frame

Xv

F v1
Xv1

Fv
Yv

Zv Yv1 Zv
Zv1
The Vehicle-2 Frame

Xv 2
F v2 Xv1

F v1

Yv1
ZvZv2 Yv1 Yv2
The Body Frame

Xv 2
F v2
Xv2
Fb Xb

Zb
Zv2 Yb Yv2
Vehicle Frame  Body Frame
, ,  are known as Euler angles. They are measured from different frames (Roll in
Fv2 frame, Pitch in Fv1 frame, Yaw in Fv frame)
Gimbal Lock
• This is a fundamental problem when
using sensors to sense Euler angles
• When pitch angle is 90 degrees, roll and
yaw rotation give the same sensor
readings
• Information for 1 dimension is lost and
the actual configuration of the rigid body
is not correctly sensed
• Solution:
– Avoid 90 degree pitch when using Euler
angle sensor
Quadrotor State Variables
• Positions in Fi : pn, pe, h
• Velocities in Fb: u, v, w
• Angular velocities in Fb: p, q, r
• Euler angles:
– Yaw angle in Fv: ψ
– Pitch angle in Fv1: θ
– Roll angle in Fv2: ϕ
Quadrotor Kinematics

• Relating position (Fi) and velocities (Fb) in


the same frame (Fi):
 pn   u  Cθ Cψ Sφ Sθ Cψ − Cφ Sψ Cφ Sθ Cψ + Sφ Sψ   u 
d   v   
 p e  = Rb  v =  Cθ Sψ Sφ Sθ Sψ + Cφ Cψ Cφ Sθ Sψ − Sφ Cψ   v 
dt
− h   w  − Sθ Sφ Cθ Cφ Cθ   w

• Relating angular velocities (Fb) to Euler


angle
 p ratesφ(F
 , F , F )  0 
v v1 v2 0
  v2()  v2 v1 ( )
 q  = R b φ  0  + R b ( φ ) R v 2 θ θ + R b ( φ ) R v 2 (θ ) R v1 (ψ )  0 
  v2 v1 v  
 r     0  ψ 
0
 p  1 0 − Sθ   φ   φ  1 Sφ Tθ Cφ Tθ   p 
 q  = 0 C        
   φ S φ C θ θ
  and θ
   = 0 C φ − S φ q 
 r  0 − Sφ Cφ Cθ  ψ  ψ  0 Sφ Cθ Cφ Cθ   r 
   
Equation of Coriolis
• Inertia frame, Fi looking at Body frame, Fb
• Vector p is moving in Fb and Fb is rotating and
translating with respect to Fi
• Time derivative of p as seen from Fi is
obtained using equation of Coriolis:

d  d   
p= p + ωb × p
dti dtb
Quadrotor Dynamics
 
dv  dv    
• Equation of Coriolis: m = m + ωb × v  = f
dti  dtb 
• m is the mass
• vector v is the velocities
• vector ωb is the angular
velocities in the body frame
• vector f is the applied forces
• In body coordinates:
 u   rv − qw   fx 
 v  =  pw − ru  + 1  f 
    m  y
 w   qu − pv   f z 
Rotational Motion
d  d    
• Equation of Coriolis for rotational motion: h= h + ωb × h = m
dti dtb
• vector h is angular momentum, h = Jωb
• J is symmetric inertia matrix I x 0 0  p τ φ 
  
J =  0 0  ; ωb =  q  ; m = τ θ 
• vector m is the applied torque 
Iy
 0 0 I z   r  τψ 
 
• Substitutes into equation of Coriolis:
 p   p   p  τ φ 
 
J  q  +  q  × J  q  = τ θ 
• Angular acceleration is hence given by:  r   r   r  τψ 

 p   {( I y − I z )qr + τ φ } I x 
 q  =  { ( I − I ) pr + τ } I 
   z x θ y

 r  {( I x − I y ) pq + τψ } I z 
Summary of Equation Set
 p n  Cθ Cψ Sφ Sθ Cψ − Cφ Sψ Cφ Sθ Cψ + Sφ Sψ   u 
 p  =  C S Sφ Sθ Sψ + Cφ Cψ

Cφ Sθ Sψ − Sφ Cψ   v 
 e  θ ψ
− h   − Sθ Sφ Cθ Cφ Cθ   w

u   rv − qw   fx 
v  =  pw − ru  + 1 f 
    m  y

w  qu − pv  
 fz 

 φ  1 Sφ Tθ Cφ Tθ   p 
   
θ  = 0 Cφ − Sφ   q 
ψ  0 Sφ Cθ Cφ Cθ   r 
  

 p   {( I y − I z )qr + τ φ } I x 
 q  =  { ( I − I ) pr + τ } I 
   z x θ y

 r  {( I x − I y ) pq + τψ } I z 
Thrust Force and Gravity Force
• fx , fy , fz are total forces acting on the body frame, Fb
• there are two components:
– quadrotor thrust force (produced by propeller)
– gravity force
• Total thrust in Fb: T = Tf + Tb +Tl + Tr
• Gravity force in Fi : (0,0,mg)
• In Fb:
 fx   0   0   0   − mgSθ 
 f  =  0  + R b R v  0  =  0  +  mgC S 
 y   v i      θ φ
 f z  − T  mg  − T  mgCθ Cφ 
Torque / Moment
• Roll : τϕ = l (Tl - Tr)
• Pitch : τθ = l (Tf - Tb)
• Yaw : τψ = τr + τl - τf - τb

• The drag of the propellers produces a


yawing torque on the body of the
quadrotor (Newton's 3rd Law)
• The direction of the torque is int he
opposite direction to the motion of the
propeller

• The thrust and torque of each motor is


controlled by its angular speed in rpm:
– Ti = kf ωi2
– τi = km ωi2
• i can take the value 1 (front motor), 2 (right
motor), 3 (back motor) and 4 (left motor)
Simplified model
 p n  Cθ Cψ Sφ Sθ Cψ − Cφ Sψ Cφ Sθ Cψ + Sφ Sψ   u 
 p  =  C S Sφ Sθ Sψ + Cφ Cψ

Cφ Sθ Sψ − Sφ Cψ   v 
 e  θ ψ

• Use vehicle 1 frame  h   Sθ

 p x 
− Sφ Cθ

 u   Cθ Sφ Sθ
− Cφ Cθ

Cφ Sθ   u 
  w

 p  = R v1 R v 2  v  =  0 
for position estimate  y
 p z 
v2 b   
 w − Sθ

Sφ Cθ
− Sφ   v 
Cφ Cθ   w

• Small Euler angles  φ  1 Sφ Tθ


  
θ  = 0 Cφ
Cφ Tθ   p   p 

− Sφ   q  =  q 

(sin, tan -> 0) ψ  0 Sφ Cθ


   Cφ Cθ   r   r 

φ  p  τ φ I x 
       fx   0   0   0   − mgSθ 
θ  =  q  = τ θ I y   f  =  0  + R b R v  0  =  0  +  mgC S 
ψ   r  τψ I z   y   v i      θ φ

     f z  − T  mg  − T  mgCθ Cφ 

• Ignore Coriolis terms  u   rv − qw   fx   fx   0   − Sθ 


 v  =  pw − ru  + 1  f  = 1  f  = 1  0  + g  C S 
 θ φ
(qr, pr, pq)
    m  y m  y m  
 w   qu − pv   f z   f z  − T  Cθ Cφ 
 
 px   Cθ Sφ Sθ Cφ Sθ   u   0   − Cφ Sθ Cψ − Sφ Sψ 
 p  =  0 Cφ
    T 
− Sφ   v  =  0  + − Cφ Sθ Sψ + Sφ Cψ 

 y  m
φ  p  τ φ I x   pz  − Sθ Sφ Cθ Cφ Cθ   w   g  
 − Cφ Cθ 

 px   0  − Cφ Sθ Cψ − Sφ Sψ 
 p  =  0  + − C S S + S C 
T      
 y   m  φ θ ψ φ ψ θ  =  q  = τ θ I y 
 pz   g   − Cφ Cθ  ψ   r  τψ I z 
     
State Estimates
• States to be measured or estimated:  φ  1 Sφ Tθ Cφ Tθ   p   p 
   
– p, q, r (from sensors)
θ  = 0 Cφ − Sφ   q  =  q 
– ϕ,θ,ψ ψ   0 Sφ Cθ Cφ Cθ   r   r 
– dot p, dot q, dot r   
– px, py, pz (from sensors)  φ   ∫ pdt 
– u, v, w θ  =  qdt 
– dot u, dot v, dot w   ∫ 
ψ   rdt 
 ∫ 
• From rate gyroscopes, we can get (p,q,r) φ  p  τ φ I x 
     
• Integrating and differentiating (p,q,r) to get θ =
     q = τ θ I y 
(ϕ,θ,ψ) and angular acceleration ψ   r  τψ I z 
   

 p x   Cθ Sφ Sθ Cφ Sθ   u 
• From position sensor (usually external  p  =  0 Cφ

− Sφ   v 
camera), we get (px, py, pz)  y 
 p z  − Sθ Sφ Cθ Cφ Cθ   w
• Differentiating position to get (u,v,w)
• From accelerometer, we get T/m, toggether  px   0  − Cφ Sθ Cψ − Sφ Sψ 
with Euler angles, we can get position  p  =  0  + − C S S + S C 
T
acceleration  y   m  φ θ ψ φ ψ
 pz   g   − Cφ Cθ 
 
Case study: From rest to hover in z
• A quadrotor is resting at its vehicle frame
• There is no rotational movement
• It starts to climb to a certain height and hovers

• From rest, the thrust is incrased


• T > mg, a is positive vertically, v increases, h increases
• T < mg, a is negative vertically, v decreases to zero, h increases
• T = mg, a is zero, v is zero, h maintains

• Exercise: From hovering to the ground


Case study: From hover to x and hover
• From hovering,
• Negative pitch, ax is positive , vx increases, x
increases
• Positive pitch, ax is negative, vx decreases to
zero, x increases
• No pitch, T = mg, a is zero, vx is zero, x is
maximum
• Exercise: How to maintain h during these
operation?
Jerk free planning
• Jerk is the time derivative of acceleration
• Physically, it is sudden start or stop
• Maximum force is upon the quadrotor with jerk

• Exercise: Qualitatively design a jerk profile such that


quadrotor is climbing up to a height to hover with jerk-free
movement.

 da d 2 v d 3 z
jz = = 2 = 3
dt dt dt