1D and 2D Quadcopter Model and Controller Design

1. Vijay Kumar
Professor
Mechanical Engineering
Chitkara University
Controller Design

2. V. K. Jadon, Prof., Mechanical Engineering, Chitkara University
Simulation
ControllerDesign – 2D
2D Model
The
Flow
…. ControllerDesign – 1D
1D Model

3. 1D Model of Quadcopter
𝑚 ሷ𝑧 𝑡 = 𝑢 𝑡 − 𝑚𝑔
𝑥1 = 𝑧(𝑡)
𝑥2 = ሶ𝑧 t
changing variables into state variables:
𝑢 𝑡 = 𝑢1
= ሶ𝑥1
In state-space modelling, we want to see the dynamics
of the each states.
ሶ𝑥1 = 𝑥2 ሶ𝑥2 =
1
𝑚
𝑢1 − 𝑔
Two initialconditionsare required to solve these first
order differential equationson the state; 𝑥1(0) and 𝑥2(0).
𝑥1 0 = 𝑧 0 = 0 𝑥2 0 = ሶ𝑧 0 = 0
𝑚𝑔
𝑏2
𝑏3
𝑎2
𝑎3
𝑢1
𝑧 = 𝑧 𝑑𝑒𝑠
scipy.integrate.odeint(func, y0, t, args=(), tfirst=False)
Function model
V K Jadon, Prof., Mechanical Engineering, Chitkara University
def model(z,t,u):
x1=z[0] # Intial Conditionfor First
FODE
x2=z[1] # Initial conditionfor
Second FODE
dx1dt=x2 # First FODE
dx2dt=(u-c*x2-k*x1)/mass-gravity # Second FODE
dxdt = [dx1dt,dx2dt]
return dxdt

4. time=np.linspace(0,1,10)
sol = odeint(model, [displacement,velocity], time)
#Initial Conditions
displacement=0, velocity=0
#System Parameters
k=0, c=0, g=9.81, mass=0.18
#System Input
u1=10
𝑘 = 0, 𝑐 = 0, 𝑔 = 9.81, 𝑚𝑎𝑠𝑠 = 0.18
𝑑 = 0; 𝑣 = 0; 𝑢1 = 0
𝑑 = 1; 𝑣 = 0;
𝑢1 = 0
𝑑 = 0; 𝑣 = 0;
𝑢1 = 1
𝑘 = 0, 𝑐 = 0, 𝑔 = 9.81, 𝑚𝑎𝑠𝑠 = 0.18
𝑘 = 0, 𝑐 = 0, 𝑔 = 9.81, 𝑚𝑎𝑠𝑠 = 0.18
𝑘 = 0, 𝑐 = 0, 𝑔 = 9.81, 𝑚𝑎𝑠𝑠 = 0.18
𝑑 = 0; 𝑣 = 5;
𝑢1 = 0
1D SODE Response
V K Jadon, Prof., Mechanical Engineering, Chitkara University
def model(z,t,u):
x1=z[0]
x2=z[1]
dx1dt=x2
dx2dt=(u-c*x2-k*x1)/mass-gravity
dxdt = [dx1dt,dx2dt]
return dxdt

5. Control Equation
𝑚𝑔
𝑦
𝑧
𝑢1
𝑧′′
𝑡 = −𝑔 +
𝑢1
𝑚
𝑡 (1)
𝑒 = 𝑧 𝑑𝑒𝑠 − 𝑧
𝑒′ = 𝑧′ 𝑑𝑒𝑠 − 𝑧′
The error term 𝑒 and 𝑒′ will decay exponentiallyto zero if error
function satisfies the following second order differential equation:
𝑒′′ + 𝑘 𝑑 𝑒′ + 𝑘 𝑝 𝑒 = 0
(𝑧 𝑑𝑒𝑠
′′
−𝑧′′
) + 𝑘 𝑑 𝑒′
+ 𝑘 𝑝 𝑒 = 0
Substitutingfrom Eqn (1)
𝑧 𝑑𝑒𝑠
′′
+ 𝑔 −
𝑢1 𝑡
𝑚
+ 𝑘 𝑑 𝑒′ + 𝑘 𝑝 𝑒 = 0
𝑢1 𝑡 = 𝑚(𝑧 𝑑𝑒𝑠
′′
+ 𝑔 + 𝑘 𝑑 𝑒′ + 𝑘 𝑝 𝑒)
The above equationis the controller equation.
𝑒, 𝑒′ 𝑢1
PD Controller
The input to the
controller is the current
and desired positions.
The output is the
control signal.
V K Jadon, Prof., Mechanical Engineering, Chitkara University

6. V K Jadon, Prof., Mechanical Engineering, Chitkara University
Interactive Mode Plotting
Interactivemode updates a plot each time a new command is issued.
Turn on interactivemode with method: plt.ion()
Turn off interactivemode with method: plt.ioff()
When interactivemode is not on, you enter all pyplot commands and then use the
method plt.show() to see the figure.
plt.clf() clears the entire current figure with all its axes, but leaves the window
opened, such that it may be reused for other plots.
plt.close() closes a window, which will be the current window, if not specified
otherwise.

7. 𝑚𝑔
𝑏2
𝑏3
𝑎2
𝑎3
∅
𝑢1 𝑐𝑜𝑠∅
𝑢1 𝑠𝑖𝑛∅
𝑢1
𝑢2
𝑚 ሷ𝑦 = −𝑠𝑖𝑛∅𝑢1
𝑚 ሷ𝑧 = 𝑐𝑜𝑠∅𝑢1 − 𝑚𝑔
𝐼 𝑥𝑥
ሷ∅ = 𝑢2
ሷ𝑦
ሷ𝑧
ሷ∅
=
0
−𝑔
0
+
−
1
𝑚
𝑠𝑖𝑛∅ 0
1
𝑚
𝑐𝑜𝑠∅ 0
0
1
𝐼 𝑥𝑥
𝑢1
𝑢2
For every equation,we will have two states
𝑥1 = 𝑦; 𝑥2 = 𝑧; 𝑥3 = ∅; 𝑥4 = ሶ𝑦; 𝑥5 = ሶ𝑧; 𝑥6 = ሶ∅;
First order Differential Equationsare
ሶ𝑥1 = ሶ𝑦 = 𝑥4 ሶ𝑥2 = ሶ𝑧 = 𝑥5 ሶ𝑥3 = ሶ∅ = 𝑥6
ሶ𝑥4 = ሷ𝑦 = −
1
𝑚
𝑠𝑖𝑛∅𝑢1 ሶ𝑥5 = ሷ𝑧 =
1
𝑚
𝑐𝑜𝑠∅𝑢1 − g ሶ𝑥6 = ሷ∅ =
𝑢2
𝐼 𝑥𝑥
2D Planar Model
ሶ𝑥1 = ሶ𝑦 + 0𝑢1 + 0𝑢2
ሶ𝑥2 = ሶ𝑧 + 0𝑢1 + 0𝑢2
ሶ𝑥3 = ሶ∅ + 0𝑢1 + 0𝑢2
ሶ𝑥4 = 0 −
1
𝑚
𝑠𝑖𝑛∅𝑢1 + 0𝑢2
ሶ𝑥5 = −𝑔 +
1
𝑚
𝑐𝑜𝑠∅𝑢1 + 0𝑢2
ሶ𝑥6 = 0 +
1
𝐼 𝑥𝑥
𝑢1 + 0𝑢2
V K Jadon, Prof., Mechanical Engineering, Chitkara University
Expanded general form

8. ሶ𝑥1
ሶ𝑥2
ሶ𝑥3
ሶ𝑥4
ሶ𝑥5
ሶ𝑥6
=
𝑥4
𝑥5
𝑥6
0
−g
0
+
0 0
0 0
0 0
−
1
𝑚
𝑠𝑖𝑛∅ 0
1
𝑚
𝑐𝑜𝑠∅ 0
0
1
𝐼 𝑥𝑥
+
𝑢1
𝑢2
𝑚 ሷ𝑦 = −∅𝑢1
𝑚 ሷ𝑧 = 𝑢1 − 𝑚𝑔
𝐼 𝑥𝑥
ሷ∅ = 𝑢2
2D Planar Model
Linearization
ሶ𝑥1
ሶ𝑥2
ሶ𝑥3
ሶ𝑥4
ሶ𝑥5
ሶ𝑥6
=
𝑥4
𝑥5
𝑥6
0
−g
0
+
0 0
0 0
0 0
−
1
𝑚
∅ 0
1
𝑚
0
0
1
𝐼 𝑥𝑥
+
𝑢1
𝑢2
The model and controller is designed for hover configuration
as it is more close to linear situation
𝑦 𝑑𝑒𝑠 = 𝑦0;
𝑢1 = 𝑚𝑔; 𝑢2 = 0;
𝑧 𝑑𝑒𝑠 = 𝑧0; ∅ 𝑑𝑒𝑠 = 0
ሷ𝑦 = −𝑔∅
(1)
ሷ𝑧 = −𝑔 +
𝑢1
𝑚
(2)
ሷ∅ =
𝑢2
𝐼 𝑥𝑥
(3)
Linearizationof the model for the hover becomes
V K Jadon, Prof., Mechanical Engineering, Chitkara University
∅ ≈ 0

9. Linearized planer model is given by
𝑦′′
= −𝑔∅
..(1)
To solve these, transform into first order differential
equationsby defining the state variablesas below
𝑥1 = 𝑦(𝑡) 𝑥2 = 𝑧(𝑡) 𝑥3 = ∅(𝑡)
𝑥′1 = 𝑦′ 𝑡 = 𝑥4 … (4)
𝑥′2 = 𝑧′ 𝑡 = 𝑥5 … (5)
𝑥′3 = ∅′ 𝑡 = 𝑥6 … (6)
SubstitutingEqn (4) into Eqn (1)
𝑥′4 = −𝑔∅ …(7)
SubstitutingEqn (5) into Eqn (2)
𝑥′5 = −𝑔 +
𝑢1
𝑚
… (8)
SubstitutingEqn (6) into Eqn (3)
𝑥′6 =
𝑢2
𝐼 𝑥𝑥
… (9)
The solutionof FODE (4) to (9) gives six states variables.
𝑥1, 𝑥2, 𝑥3, 𝑥4, 𝑥5, 𝑥6
These are positions(𝑦, 𝑧, ∅) and their time
derivatives(𝑦′, 𝑧′, ሶ∅′ )
Position and Time Derivatives
𝑧′′
= −𝑔 +
𝑢1
𝑚
… (2)
∅′′
=
𝑢2
𝐼 𝑥𝑥
… (3)
V K Jadon, Prof., Mechanical Engineering, Chitkara University
changing dots (.) with
dashes (‘) for convenience
We need 𝑢1 and 𝑢2 to solve these set of FODE
𝑢1 and 𝑢2 are output of control equation.
We need to design controllerfor getting
appropriatevaluesof 𝑢1 and 𝑢2

10. 𝑢1, 𝑢2 𝑦, 𝑧, ∅
The error (desired value-currentvalue) in roll angle and position
will reach to zero only if the error function satisfies the second
order differential equation.
𝑒′′ + 𝑘 𝑑 𝑒′ + 𝑘 𝑝 𝑒 = 0
𝑦 𝑑𝑒𝑠
′′
− 𝑦′′
+ 𝑘 𝑑𝑦 𝑒 𝑦
′
+ 𝑘 𝑝𝑦 𝑒 𝑦 = 0
Controller
𝑧 𝑑𝑒𝑠
′′
− 𝑧′′
+ 𝑘 𝑑𝑧 𝑒 𝑧
′
+ 𝑘 𝑝𝑧 𝑒 𝑧 = 0
∅ 𝑐
′′ − ∅′′ + 𝑘 𝑑∅ 𝑒∅
′
+ 𝑘 𝑝∅ 𝑒∅ = 0
𝑦 −Transllation
Controller
𝑜𝑢𝑡𝑝𝑢𝑡𝑦, 𝑦 𝑑𝑒𝑠
Thrust
Controller
𝑧, 𝑧 𝑑𝑒𝑠
Roll Controller
∅, ∅ 𝑐
𝑜𝑢𝑡𝑝𝑢𝑡
𝑜𝑢𝑡𝑝𝑢𝑡
V K Jadon, Prof., Mechanical Engineering, Chitkara University

11. 𝑦 −Transllation
Controller
∅ 𝑐𝑦, 𝑦 𝑑𝑒𝑠
Thrust
Controller
𝑢1𝑧, 𝑧 𝑑𝑒𝑠
Roll Controller
𝑢2∅, ∅ 𝑐
The linearized planer model
𝑦′′ = −𝑔∅
..(1)
𝑧′′ = −𝑔 +
𝑢1
𝑚
… (2)
∅′′ =
𝑢2
𝐼 𝑥𝑥
… (3)
𝑦 𝑑𝑒𝑠
′′
− 𝑦′′
+ 𝑘 𝑑𝑦 𝑒 𝑦
′
+ 𝑘 𝑝𝑦 𝑒 𝑦 = 0
Substitutingfrom Eqn (1)
𝑦 𝑑𝑒𝑠
′′
+ 𝑔∅ 𝑐 + 𝑘 𝑑𝑦 𝑒 𝑦
′
+ 𝑘 𝑝𝑦 𝑒 𝑦 = 0
∅ 𝑐 = −
1
𝑔
(𝑦 𝑑𝑒𝑠
′′
+ 𝑘 𝑑𝑦 𝑒 𝑦
′
+ 𝑘 𝑝𝑦 𝑒 𝑦) ..(10)
𝑧 𝑑𝑒𝑠
′′
− 𝑧′′
+ 𝑘 𝑑𝑧 𝑒 𝑧
′
+ 𝑘 𝑝𝑧 𝑒 𝑧 = 0
Substitutingfrom Eqn (2)
𝑧 𝑑𝑒𝑠
′′
+ 𝑔 −
𝑢1
𝑚
+ 𝑘 𝑑𝑧 𝑒 𝑧
′ + 𝑘 𝑝𝑧 𝑒 𝑧
𝑢1 = 𝑚(𝑧 𝑑𝑒𝑠
′′
+ 𝑔 + 𝑘 𝑑𝑧 𝑒 𝑧
′
+ 𝑘 𝑝𝑧 𝑒 𝑧)…(11)
∅ 𝑐
′′ − ∅′′ + 𝑘 𝑑∅ 𝑒∅
′
+ 𝑘 𝑝∅ 𝑒∅ = 0
Substitutingfrom Eqn (3)
∅ 𝑐
′′
−
𝑢2
𝐼 𝑥𝑥
+ 𝑘 𝑑∅ 𝑒∅
′
+ 𝑘 𝑝∅ 𝑒∅ = 0
𝑢2 = 𝐼 𝑥𝑥 ∅ 𝑐
′′
+ 𝑘 𝑑∅ 𝑒∅
′
+ 𝑘 𝑝∅ 𝑒∅ … (12)
Control Equations
The SODE for error to satisfy
V K Jadon, Prof., Mechanical Engineering, Chitkara University
𝑒 𝑦
′
= (𝑦 𝑑𝑒𝑠
′
− 𝑦′) 𝑒 𝑦 = (𝑦 𝑑𝑒𝑠 − 𝑦)
𝑒 𝑧
′
= (𝑧 𝑑𝑒𝑠
′
− 𝑧′) 𝑒 𝑧 = (𝑧 𝑑𝑒𝑠 − 𝑧)
𝑒∅
′
= (∅ 𝑐
′
− ∅′) 𝑒∅ = (∅ 𝑐 − ∅)
∅ 𝑐
′

12. V K Jadon, Prof., Mechanical Engineering, Chitkara University
In this problem we have three degrees of freedom (𝑦, 𝑧, ∅) and two control
variables(𝑢1, 𝑢2).
So, we need cascaded control system.
To reach the desired position we have to command a roll angle (∅ 𝑐) such
that required translation(𝑦 𝑑𝑒𝑥, 𝑧 𝑑𝑒𝑠) is achieved.
Position
Controller
∅ 𝑐
∅′ 𝑐
𝑦 𝑑𝑒𝑠, 𝑦′ 𝑑𝑒𝑠
𝑧 𝑑𝑒𝑠, 𝑧′ 𝑑𝑒𝑠
Roll
Controller
𝑢2
𝑢1
𝑦, 𝑧, ∅
𝑢1
Current states
𝑦′, 𝑧′, ∅′
∅, ∅′
Control Block

13. Thanks
V K Jadon, Prof., Mechanical Engineering, Chitkara University 13