Download - Lecture 1: Quadrotor
Chapter 3Quadrotor / 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)
• Back motor (Mb) (-x)
• Right motor (Mr) (+y)
• Left motor (Ml) (-y)
• 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
• Each spinning motor provides – thrust force (T) for lifting– torque () for rotating
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
The inertia frame
• 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
X (North)
Y (East)
Z (Down)
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).
Xi (North)
Yi (East)
Zi (Down)
Xv
Yv
Zv
F v
F i
The Vehicle-1 Frame
Xv
Yv
Zv
F v
F v1
Zv
Zv1
Yv1
Xv1
The Vehicle-2 Frame
Xv 2
Zv2Zv
F v1
F v2
Yv1 Yv2 Yv1
Xv1
The Body Frame
Xv 2
Zv2
Zb
F v2
F b
Xv2
Xb
Yv2Yb
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
−−++−
=
=
− w
v
u
CCCSS
CSSSCCCSSSSC
SSCSCSCCSSCC
w
v
u
R
h
p
p
dt
d vbe
n
θφθφθ
ψφψθφψφψθφψθ
ψφψθφψφψθφψθ
( ) ( ) ( ) ( ) ( ) ( )
−=
−
−=
+
+
=
r
q
p
CCCS
SC
TCTS
CCS
CSC
S
r
q
p
RRRRRR
r
q
pvv
vv
bv
vv
bv
bv
θφθφ
φφ
θφθφ
θφφ
θφφ
θ
ψθφ
ψθφ
ψψθφθθφ
φφ
0
0
1
and
0
0
01
0
0
0
0
0
0 1212
2122
• Relating position (Fi) and velocities (Fb) in the same frame (Fi):
• Relating angular velocities (Fb) to Euler angle rates (Fv, Fv1, Fv2)
Equation of Coriolis
ppdt
dp
dt
db
bi
×+= ω
• 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:
Quadrotor Dynamics
• Equation of Coriolis:• 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:
+
−−
−=
z
y
x
f
f
f
mpvqu
rupw
qwrv
w
v
u1
fvdt
vdm
dt
vdm b
bi
=
×+= ω
Rotational Motion
• Equation of Coriolis for rotational motion:
• vector h is angular momentum, h = Jωb
• J is symmetric inertia matrix• vector m is the applied torque
• Substitutes into equation of Coriolis:
• Angular acceleration is hence given by:
mhhdt
dh
dt
db
bi
=×+= ω
=
=
=
ψ
θ
φ
τττ
ω m
r
q
p
I
I
I
J b
z
y
x
; ;
00
00
00
{ }{ }{ }
+−+−+−
=
=
×
+
zyx
yxz
xzy
IpqII
IprII
IqrII
r
q
p
r
q
p
J
r
q
p
r
q
p
J
ψ
θ
φ
ψ
θ
φ
τττ
τττ
)(
)(
)(
Summary of Equation Set
−−++−
=
− w
v
u
CCCSS
CSSSCCCSSSSC
SSCSCSCCSSCC
h
p
p
e
n
θφθφθ
ψφψθφψφψθφψθ
ψφψθφψφψθφψθ
−=
r
q
p
CCCS
SC
TCTS
θφθφ
φφ
θφθφ
ψθφ
0
0
1
{ }{ }{ }
+−+−+−
=
zyx
yxz
xzy
IpqII
IprII
IqrII
r
q
p
ψ
θ
φ
τττ
)(
)(
)(
+
−−
−=
z
y
x
f
f
f
mpvqu
rupw
qwrv
w
v
u1
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:
−+
−=
+
−=
φθ
φθ
θ
CmgC
SmgC
mgS
Tmg
RR
Tf
f
fvi
bv
z
y
x
0
0
0
0
0
0
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 ωi
2
– τi = km ωi2
• i can take the value 1 (front motor), 2 (right motor), 3 (back motor) and 4 (left motor)
Simplified model
• Use vehicle 1 frame for position estimate
• Small Euler angles (sin, tan -> 0)
• Ignore Coriolis terms (qr, pr, pq)
−−−++−
=
w
v
u
CCCSS
CSSSCCCSSSSC
SSCSCSCCSSCC
h
p
p
e
n
θφθφθ
ψφψθφψφψθφψθ
ψφψθφψφψθφψθ
−+
−=
=
+
−−
−=
φθ
φθ
θ
CC
SC
S
g
Tm
f
f
f
mf
f
f
mpvqu
rupw
qwrv
w
v
u
z
y
x
z
y
x
0
0111
=
−=
r
q
p
r
q
p
CCCS
SC
TCTS
θφθφ
φφ
θφθφ
ψθφ
0
0
1
=
=
z
y
x
I
I
I
r
q
p
ψ
θ
φ
τττ
ψθφ
−−=
=
w
v
u
CCCSS
SC
SCSSC
w
v
u
RR
p
p
pvb
vv
z
y
x
θφθφθ
φφ
θφθφθ
0212
−+−−−
+
=
−−=
θφ
ψφψθφ
ψφψθφ
θφθφθ
φφ
θφθφθ
CC
CSSSC
SSCSC
m
T
gw
v
u
CCCSS
SC
SCSSC
p
p
p
z
y
x
0
0
0
−+
−=
+
−=
φθ
φθ
θ
CmgC
SmgC
mgS
Tmg
RR
Tf
f
fvi
bv
z
y
x
0
0
0
0
0
0
=
=
z
y
x
I
I
I
r
q
p
ψ
θ
φ
τττ
ψθφ
−+−−−
+
=
θφ
ψφψθφ
ψφψθφ
CC
CSSSC
SSCSC
m
T
gp
p
p
z
y
x
0
0
State Estimates• States to be measured or estimated:
– p, q, r (from sensors)– ,θ,ψϕ– dot p, dot q, dot r– px, py, pz (from sensors)– u, v, w– dot u, dot v, dot w
• From rate gyroscopes, we can get (p,q,r)• Integrating and differentiating (p,q,r) to get
( ,θ,ψ) and angular accelerationϕ
• From position sensor (usually external camera), we get (px, py, pz)
• Differentiating position to get (u,v,w)• From accelerometer, we get T/m, toggether
with Euler angles, we can get position acceleration
=
=
z
y
x
I
I
I
r
q
p
ψ
θ
φ
τττ
ψθφ
−+−−−
+
=
θφ
ψφψθφ
ψφψθφ
CC
CSSSC
SSCSC
m
T
gp
p
p
z
y
x
0
0
=
−=
r
q
p
r
q
p
CCCS
SC
TCTS
θφθφ
φφ
θφθφ
ψθφ
0
0
1
−−=
w
v
u
CCCSS
SC
SCSSC
p
p
p
z
y
x
θφθφθ
φφ
θφθφθ
0
=
∫∫∫
rdt
qdt
pdt
ψθφ
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.
3
3
2
2
dt
zd
dt
vd
dt
adjz
===