Guidance and Control of Unmanned Autonomous Vehicles

Shashi Ranjan Kumar

Assistant Professor
Department of Aerospace Engineering
Indian Institute of Technology Bombay
Powai, Mumbai- 400076

https://www.aero.iitb.ac.in/˜shashi
srk@aero.iitb.ac.in
Quadrotor: Euler angles based Approach, Horizontal Position Channel

• Horizontal position control:

lim ∥eph (t)∥ = 0, eph (t) = ph (t) − phd (t)

t→∞

• How to design controller now?

ëph + Kphd ėph + Kphp eph = 0 =⇒ p̈h = p̈hd − Kphd ėph − Kphp eph

• As p̈h = −gAψ Θh , we have

−gAψ Θhd = p̈hd − Kphd ėph − Kphp eph

=⇒ Θhd = −g −1 A−1


ψ p̈hd − Kphd ėph − Kphp eph

• If Θh = Θhd then limt→∞ ∥eph (t)∥ = 0

• For a set-point control problem, p̈hd = ṗhd = 0 then

Θhd = −g −1 A−1
 
ψ −Kphd ṗh − Kphp (ph − phd )

Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 1 / 14
Quadrotor: Euler angles based Approach, Altitude Channel

• Altitude Channel: Control objective:

lim |epz (t)| = 0, epz (t) = pz − pzd

t→∞

• Control design?

ëpz + Kpzd ėpz + Kpzp epz = 0 =⇒ p̈z = p̈zd − Kpzd ėpz − Kpzp epz

• How to proceed now?

fd
g− = p̈zd − Kpzd ėpz − Kpzp epz
m
 
=⇒ fd = mg − m p̈zd − Kpzd ėpz − Kpzp epz

• If f = fd then limt→∞ ∥epz (t)∥ = 0

• For a set-point control problem, p̈zd = ṗzd = 0 then
 
fd = mg − m −Kpzd ṗz − Kpzp (pz − pzd )

Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 2 / 14
Quadrotor: Euler angles based Approach, Horizontal Position Channel, PID

• To satisfy limt→∞ ∥eph (t)∥ = 0 using ṗh = vh , desired value of vh

vhd = Kph (phd − ph )

• With assumption of ṗhd = 0 and evh = vh − vhd then

lim ∥evh (t)∥ = 0 =⇒ lim ∥eph (t)∥ = 0

t→∞ t→∞

• As v̇h = −gAψ Θh , we choose PID controller as

Z
−gAψ Θhd = −Kvhp evh − Kvhi evh − Kvhd ėvh

• With assumption of v̇hd = 0,

lim |Θh (t) − Θhd (t)| = 0 =⇒ lim |evh (t)| = 0

t→∞ t→∞

• Desired attitude angle

Θhd = g −1 A−1
 R 
ψ Kvhp evh + Kvhi evh + Kvhd ėvh

Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 3 / 14
Quadrotor: Euler angles based Approach, Horizontal Position Channel, PID

• What do we expect from attitude controller?

• Attitude angle controller objective: To ensure that

lim ∥Θh (t) − Θhd (t)∥ = 0

t→∞

• As a result,
lim ∥eph ∥ = 0
t→∞

• For a set point control problem,

p̈hd = ṗhd = 0

• Kvhd ėvh can be omitted to avoid noise by differential mechanism.

• Why only P-controller for position but PID for velocity?
• First one is based on kinematic model (uncertainty free) but second one is
using dynamic model, subject to uncertainties.
• PID can compensate in better manner for uncertainties in systems.

Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 4 / 14
Quadrotor: Euler angles based Approach, Altitude Channel, PID

• To satisfy limt→∞ |epz (t)| = 0 using ṗz = vz , desired value of vz

vzd = Kpz (pzd − pz )

• With assumption of ṗz = 0 and evz = vz − vzd then

lim |evz (t)| = 0 =⇒ lim |epz (t)| = 0
t→∞ t→∞

f
• As v̇h = g − , we choose PID controller given by
m
Z
fd
g− = −Kvzp evz − Kvzi evz − Kvzd ėvz
m
• With v̇zd = 0, limt→∞ |f (t) − fd (t)| = 0 =⇒ limt→∞ |evz (t)| = 0
• Desired thrust
R

fd = m g + Kvzp evz + Kvzi evz + Kvzd ėvz

• For set-point problem, ṗzd = p̈zd = 0 and the derivative term Kvzd ėvz can
be omitted to reduce the noise in input.
Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 5 / 14
Quadrotor: Euler angles based Approach, Horizontal Position Channel

• How to deal with large errors in position or altitude of vehicle?

• Will small assumptions used earlier in dynamics still hold?
• Desired command may be unacceptable, and may even lead to crash of
multicopters.
• How to address these issues?
• Desired command in such case, for dynamic inversion based design

Θhd = satgd −g −1 A−1

   
ψ −Kphd ṗh − Kphp (ph − phd ) , a0 , a0 ∈ R+


u ∥u∥∞ ≤ a
satgd (u, a) ≜ u
a ∥u∥∞ > a
∥u∥∞
• For PID-based autopilot, evh = satgd (vh − vhd , a1 ) , a1 ∈ R+

Θhd = satgd g −1 A−1

 R 

ψ −Kvhp evh − Kvhi evh − Kvhd ėvh , a2 , a2 ∈ R+

Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 6 / 14
Quadrotor: Euler angles based Approach, Horizontal Position Channel

• How is this saturation function different from conventional one?

 
sat(u1 , a)
(
uk |u|k ≤ a
sat(u, a) ≜  ...  , sat(uk , a) ≜
a sign(uk ) |uk | > a
sat(un , a)

• What would happen to the directions of output vector w.r.t. input?

• Direction-guaranteed saturation function (satgd ) guarantees that the

absolute value of each element of final vector ≤ input vector.
• Also, the direction is guaranteed to be the same as that of input.

Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 7 / 14
Quadrotor: Euler angles based Approach, Altitude Channel

• To avoid saturation on throttle input,




fd = satgd m g + m Kpzd ṗz + Kpzp (pz − pzd ) , a3 , a3 ∈ R+

• In case of PID-based design, evz = satgd (vz − vzd , a4 ), a4 ∈ R+

R


=⇒ fd = satgd m g + Kvzp evz + Kvzi evz + Kvzd ėvz , a5 , a5 ∈ R+

• For scalar case, direction-guaranteed saturation function is the same as the

traditional saturation function.
• What if constraint is fd ∈ [fmin , fmax ]?
  Z 
fmin + fmax
fd = + satgd m g + Kvzp evz + Kvzi evz + Kvzd ėvz
2
fmin + fmax fmax − fmin
i
− ,
2 2

Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 8 / 14
Quadrotor: Rotation Matrix based Approach

• How to perform design based on rotation matrix?

• Recall Ṙ = R[ω]×
• Desired rotation matrix Rd obtained through position control.
• Model for quadrotor

e e e f e
ṗ = v, v̇ = ge3 − Rb e3 , Ṙ = R[ v ω]×
m
• Assume that R = [r1 r2 r3 ] and Rd = [r1,d r2,d r3,d ]
fd
ge3 − r3,d = ad , r3,d = Rd e3
m
Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 9 / 14
Quadrotor: Rotation Matrix based Approach

• Desired acceleration can be obtained, as in the previous case

 R


satgd −Kvhp evh − Kvhi evh − Kvhd ėvh , a6
ad = R


satgd −Kvzp evz − Kvzi evz − Kvzd ėvz , a7

• Can we obtain third column of the rotation matrix?

fd m(ge3 − ad )
ge3 − r3,d = ad =⇒ r3,d =
m fd

⊤
• As r3,d r3,d = 1, due to orthogonality R, corrected version is

ge3 − ad
r3,d =
∥ge3 − ad ∥

• We need to determine other columns to get complete rotation matrix.

• Do we need all three columns of R?
• Determining one more column is enough due to orthogonality conditions.

Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 10 / 14
Quadrotor: Rotation Matrix based Approach, Small Angle Case

• First column of the rotation matrix

 
cos θ cos ψ
r1 =  cos θ sin ψ 
− sin θ

• With small angle assumptions,

 
cos ψ
r̄1,d =  sin ψ 
0

• As second column is orthogonal to both first and third columns,

r3,d × r̄1,d
r2,d = ⊥ r3,d
∥r3,d × r̄1,d ∥
• If we define r1,d = r2,d × r3,d then rotation matrix

Rd = [r2,d × r3,d r2,d r3,d ], R⊤

d Rd = I3

• What if the small angle assumptions do not hold?

Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 11 / 14
Quadrotor: Rotation Matrix based Approach, Large Angle Case

• Third column of the rotation matrix

   
cos ψd sin θd cos ϕd + sin ψd sin ϕd a11
r3,d =  sin ψd sin θd cos ϕd − cos ψd sin ϕd  =  a12 
cos ϕd cos θd a13

• Solve for θd , ϕd

sin ϕd = a11 sin ψd − a12 cos ψd

sin θd cos ϕd = a11 cos ψd + a12 sin ψd
cos ϕd cos θd = a13

• Solutions
 
a11 cos ψd + a12 sin ψd −(a11 cos ψd + a12 sin ψd )
 
θd = tan−1 , tan−1
a13 −a13

ϕd = sin−1 (a11 sin ψd − a12 cos ψd ) , π − sin−1 (a11 sin ψd − a12 cos ψd )
• How to select one of these two values of Euler angles?

Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 12 / 14
Quadrotor: Rotation Matrix based Approach

• Using continuity of Euler angles, we can determine one of these values.

• Rotation matrix obtained using Euler angles.
• How to obtain desired thrust in this approach?
fd
ge3 − r3,d = ad =⇒ fd r3,d = mge3 − mad
m
• How to proceed further to obtain thrust?
⊤
• Pre-multiply with r3,d on both sides

⊤ ge3 − ad
fd = mr3,d (ge3 − ad ) , r3,d =
∥ge3 − ad ∥
• For constrained throttle input,
fmin + fmax fmax − fmin fmin + fmax
h i
⊤
fd = satgd mr3,d (ge3 − ad ) − , +
2 2 2

Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 13 / 14
References

References

• Quan Quan, “Introduction to Multicopter Design and Control”,

Springer, 2017.
• Moussa Labbadi,“Advanced Robust Nonlinear Control Approaches
for Quadrotor Unmanned Aerial Vehicle”, 2021.

Thank you for your attention !!!

Shashi Ranjan Kumar (IITB) Guidance and Control of Unmanned Autonomous Vehicles Quadrotor Lecture 14 14 / 14