![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](https://image.slidesharecdn.com/session-viquadmodel-200110053538/85/quadcopter-modelling-and-controller-design-3-320.jpg)
![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](https://image.slidesharecdn.com/session-viquadmodel-200110053538/85/quadcopter-modelling-and-controller-design-4-320.jpg)
quadcopter modelling 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