AE4610
  • Welcome
  • Experiments
  • Lab 0: MATLAB & SIMULINK
  • Lab 1: Rotary Servo Base
    • A. Integration (Week 1)
    • B. Modeling (Week 1)
    • C. Control Design (Week 2)
    • D. Controller Implementation & Evaluation (Week 3)
  • Lab 2: 3 DOF Gyroscope
    • A & B. Modeling and Control Design (Week 1)
    • C. Controller Implementation (Week 2)
  • Lab 3: Rotary Flexible Link
    • A. System Identification (Week 1)
    • B & C. Control Design and Controller Implementation (Week 2)
  • Lab 4: Rotary Inverted Pendulum
    • A & B. Modeling and Balance Control
    • C. Swing-up Control (Demo)
  • Final Project
  • Archive
    • Old Lab 1 Model Validation
    • Problem Set
    • DC Motor
    • Lab 1: Rotary Servo Base (Older Version)
      • A. Integration (Week 1)
      • B. Modelling (Week 1)
      • C. Position Control (Week 2)
    • Quadcopter
    • Gyroscope
    • Inverted Pendulum
    • 3DOF Helicopter
    • Torsional Pendulum
    • Aero LQR
      • A. System Identification
      • A. System Identification (Week 1) last check
      • B. Control Design & Implementation (Week 2)
    • Rotary Gyro
      • Rotary Gyro
    • 2 DOF AERO
      • A. System Identification
      • B. Control Design
      • C. Controller Implementation
    • Copy of Lab 4: Rotary Inverted Pendulum
      • Week 1
      • Week 2
Powered by GitBook
On this page
  • Introduction
  • Background
  • Pitch Stiffness
  • Equations of Motion
  • First-Order Response
  • Second-Order Response
  • Estimating the Damping Ratio, Natural Frequency and Time Constant
  • Estimating the Thrust Parameters
  • System Identification
  • Experimental Steps for Finding System Parameters
  • Analysis
  • Results for Report
  • Questions for Report

Was this helpful?

Export as PDF
  1. Archive
  2. 2 DOF AERO

A. System Identification

Previous2 DOF AERONextB. Control Design

Last updated 1 year ago

Was this helpful?

Introduction

The Quanser Aero Experiment can be configured as a conventional dual-rotor helicopter, as shown in Figure 1a. The front rotor that is horizontal to the ground predominantly affects the motion about the pitch axis while the back or tail rotor mainly affects the motion about the yaw axis (about the vertical shaft).

The tail rotor in helicopters is also known as the anti-torque rotor because it is used to reduce the torque that the main rotor generates about the yaw. Without this, the helicopter would be difficult to stabilize about the yaw axis. The rotors on the Quanser Aero Experiment are the same size and equidistant from the vertical shaft, the tail rotor also generates a torque about the pitch axis. As a result, both the front and back/tail rotors generate torques on each other.

Background

Pitch Stiffness

The Quanser 2D AERO is designed with its mass distribution well balanced from the front and back rotors such that its center of gravity is at the mid-point between the two rotors. However, a vertical offset of the cg location is included to result in pitch stiffness from the pendulum effect as depicted in Figure 1b.

The system is in static equilibrium when it is horizontal and parallel to the ground with zero pitch angle. Any change in pitch angle θb\theta_bθb​ gives rise to a restoring moment from the pendulum effect given by restoring pitch moment due to pitch angle change = −MbgDmsin⁡(θb)≈−MbgDmθb-M_bgD_m \sin(\theta_b)\approx -M_bgD_m \theta_b−Mb​gDm​sin(θb​)≈−Mb​gDm​θb​

Equations of Motion

The free-body diagram of the Quanser Aero Experiment is illustrated in Figure 2.

The following conventions are used for the modeling:

  • The helicopter is horizontal and parallel with the ground when the pitch angle is zero, i.e., θ=0\theta=0θ=0 .

  • The pitch angle increases positively, θ(t)>0\theta(t)>0θ(t)>0 , when the front rotor is moved upwards and the body rotates clockwise (CW) about the Y axis.

  • The yaw angle increases positively, ψ(t)>0\psi(t)>0ψ(t)>0 , when the body rotates counter-clockwise (CCW) about the Z axis.

  • Pitch increases, θ>0\theta>0θ>0 , when the front rotor voltage is positive Vθ>0V_\theta>0Vθ​>0 .

  • Yaw increases, ψ>0\psi>0ψ>0 , when the back (or tail) rotor voltage is positive, Vψ>0V_{\psi}>0Vψ​>0 .

When voltage is applied to the pitch motor, VθV_\thetaVθ​ , the speed of rotation results in a force, FpF_pFp​ that acts normal to the body at a distance rpr_prp​ from the pitch axis as shown in Figure 2. The rotation of the propeller generates a torque about the pitch rotor motor shaft which is in turn seen about the yaw axis. Thus, rotating the pitch propeller does not only cause motion about the pitch axis but also about the yaw axis. As described earlier, that is why conventional helicopters include a tail, or anti-torque, rotor to compensate for the torque generated about the yaw axis by the large, main rotor.

Similarly, the yaw motor causes a force FyF_yFy​ that acts on the body at a distance ryr_yry​ from the yaw axis as well as a torque about the pitch axis as shown in Figure 2.

The equations of motion can be approximated as:

For pitch axis:

θ¨+2ζθωnθθ˙+ωnθ2θ=Tθ(1)\ddot{\theta} + 2\zeta_\theta\omega_{n_\theta} \displaystyle \dot{\theta} + \omega_{n_\theta}^2 \theta = \Tau_{\theta} \qquad \qquad \tag{1}θ¨+2ζθ​ωnθ​​θ˙+ωnθ​2​θ=Tθ​(1)

For yaw axis:

ψ¨+1τψψ˙=Tψ(2)\ddot{\psi}+ \frac{1}{\tau_\psi} \dot{\psi}=\Tau_\psi \qquad \qquad \tag{2}ψ¨​+τψ​1​ψ˙​=Tψ​(2)

where the torques acting on the pitch and yaw axes are

Tθ=K‾θθVθ+K‾θψVψ(3)\Tau_{\theta} = \overline{K}_{\theta \theta} V_{\theta} + \overline{K}_{\theta \psi} V_{\psi} \qquad \qquad \tag{3}Tθ​=Kθθ​Vθ​+Kθψ​Vψ​(3)
Tψ=K‾ψθVθ+K‾ψψVψ(4)\Tau_\psi = \overline{K}_{\psi \theta} V_{\theta} + \overline{K}_{\psi \psi} V_{\psi} \qquad \qquad \tag{4}Tψ​=Kψθ​Vθ​+Kψψ​Vψ​(4)

The parameters used in the EOMs above are:

  • ζθ\zeta_\thetaζθ​ : the damping ratio of the pitch dynamics

  • ωnθ\omega_{n_\theta}ωnθ​​ : the natural frequency of the pitch dynamics

  • τψ\tau_{\psi}τψ​ : the time constant of the yaw dynamics

  • K‾θθ\overline{K}_{\theta\theta}Kθθ​ : normalized torque thrust gain from the pitch rotor (normalized by JθJ_{\theta}Jθ​)

  • K‾ψψ\overline{K}_{\psi\psi}Kψψ​ : normalized torque thrust gain from the yaw rotor (normalized by JψJ_{\psi}Jψ​)

  • K‾θψ\overline{K}_{\theta\psi}Kθψ​ : normalized cross-torque thrust gain acting on the pitch from the yaw rotor (normalized by JθJ_{\theta}Jθ​)

  • K‾ψθ\overline{K}_{\psi\theta}Kψθ​ : normalized cross-torque thrust gain acting on the yaw from the pitch rotor (normalized by JψJ_{\psi}Jψ​)

  • VθV_\thetaVθ​ : voltage applied to the pitch rotor motor

  • VψV_\psiVψ​ : voltage applied to the yaw rotor motor

  • JθJ_\thetaJθ​ : the total moment of inertia about the pitch axis

  • JψJ_{\psi}Jψ​ : the total moment of inertia about the yaw axis

First-Order Response

The step response of a first-order transfer function

Y(s)=τy(0)+KU(s)τs+1(7)Y(s) = \displaystyle \frac{\tau y(0)+KU(s)}{\tau s+1} \qquad \qquad \tag{7}Y(s)=τs+1τy(0)+KU(s)​(7)

where y(0) is the initial condition, K is the DC or steady-state gain, and τ is the time constant as illustrated in Figure 3. Figure 3 is for a system with y(0) = 0, K=1K = 1K=1 rad/V and τ=0.05\tau = 0.05τ=0.05 sec.

To obtain the time constant from the response, find the time it takes to reach 1−e−11-e^{-1}1−e−1or 63.2% of its final steady-state value:

y(t1)=y1=(1−e−1)(yss−y0)+y0(8)y(t_1)=y_1=(1-e^{-1})(y_{\rm ss}-y_0) +y_0 \qquad \qquad \tag{8}y(t1​)=y1​=(1−e−1)(yss​−y0​)+y0​(8)

The time constant is τ=t1−t0\tau=t_1-t_{0 }τ=t1​−t0​, where t0t_0t0​ is the start time of the step and t1t_1t1​ is the time it takes to reach 63.2% of the final value, as illustrated in Figure 3.

Second-Order Response

The general equation of motion of a second-order system with input u(t) is described by

α¨+2ζωnα˙+ωn2α=u(t)(9)\ddot{\alpha} + 2\zeta\omega_n\dot{\alpha} + \omega_n^2\alpha = u(t) \qquad \qquad \tag{9}α¨+2ζωn​α˙+ωn2​α=u(t)(9)

Assuming the initial conditions α(0−)=α0\alpha(0-)=\alpha_0α(0−)=α0​ and α˙(0−)=0\dot\alpha(0-)=0α˙(0−)=0, the Laplace transform of Equation (9) is

α(s)=(s+2ζωn)α0+U(s)s2+2ζωns+ωn2(10)\alpha(s)=\displaystyle{\frac{(s+2\zeta\omega_n)\alpha_0 +U(s)}{s^2+2\zeta\omega_ns+\omega_n^2}} \qquad \qquad \tag{10}α(s)=s2+2ζωn​s+ωn2​(s+2ζωn​)α0​+U(s)​(10)

The characteristic equation from Equation (10) is obtained as,

s2+2ζωns+ωn2=0(11)s^2+2\zeta\omega_ns+\omega_n^2 = 0 \qquad \qquad \tag{11}s2+2ζωn​s+ωn2​=0(11)

where ζ\zetaζ is the damping ratio and ωn\omega_nωn​ is the natural frequency.

The typical response of an underdamped second order system to a step input with zero initial conditions is shown in Figure 4. The natural frequency and damping ratio can be obtained from transient response using Figure 4 as follows:

Finding the Natural Frequency

The period of the oscillations in a system response can be found using the equation

Tosc=tn−t1n−1(12)T_{\rm osc}=\displaystyle\frac{t_n-t_1}{n-1} \qquad \qquad \tag{12}Tosc​=n−1tn​−t1​​(12)

where tnt_ntn​ is the time of the nthn^{th}nth peak, t1t_1t1​ is the time of the first peak, and nnn is the number of oscillations considered. From this, the damped natural frequency (in radians per second) is

ωd=2πTosc(13)\omega_d=\displaystyle \frac{2\pi}{T_{\rm osc}} \qquad \qquad \tag{13}ωd​=Tosc​2π​(13)

and the undamped natural frequency is

ωn=ωd1−ζ2(14)\omega_n=\displaystyle\frac{\omega_d}{\sqrt{1-\zeta^2}} \qquad \qquad \tag{14}ωn​=1−ζ2​ωd​​(14)

Finding the Damping Ratio

The damping ratio of a second-order system can be found from the transient response to a step input. For a typical second-order underdamped system, the subsidence ratio (i.e. decrement ratio) is defined as

δ=1n−1ln⁡O1On(15)\delta = \displaystyle\frac{1}{n-1}\ln\frac{O_1}{O_n} \qquad \qquad \tag{15}δ=n−11​lnOn​O1​​(15)

where O1O_1O1​ is the peak magnitude of the first oscillation and OnO_nOn​ is the peak magnitude of the nthn^{th}nth oscillation. Note that O1O_1O1​ > OnO_nOn​ , as this is a decaying response. The damping ratio can then be found using

ζ=δ4π2+δ2(16)\zeta= \displaystyle\frac{\delta}{\sqrt{4\pi^2+\delta^2}} \qquad \qquad \tag{16}ζ=4π2+δ2​δ​(16)

Estimating the Damping Ratio, Natural Frequency and Time Constant

The damping ratio and the natural frequency of the pitch axis, ζθ\zeta_\thetaζθ​ and ωnθ\omega_{n_\theta}ωnθ​​ in Equation (1) can be found from the oscillatory portion of the step response.

Pitch Axis: Apply a voltage only to the pitch rotor motor and get the system response in the pitch axis. The resulting second-order equation of motion is

θ¨+2ζθωnθθ˙+ωnθ2θ=K‾θθVθ(t)(17)\ddot{\theta} + 2\zeta_\theta\omega_{n_\theta} \displaystyle \dot{\theta} + \omega_{n_\theta}^2 \theta = \overline{K}_{\theta\theta}V_{\theta}(t) \qquad \qquad \tag{17}θ¨+2ζθ​ωnθ​​θ˙+ωnθ​2​θ=Kθθ​Vθ​(t)(17)

Taking its Laplace transform gives

θ(Θ(s)s2−sθ(0−)−θ˙(0−))+2ζθωnθ(Θ(s)−θ(0−))+ωnθ2Θ(s)=K‾θθVθ(s)(18)\theta\left(\Theta(s)s^2-s\theta(0^-)-\dot{\theta}(0^-)\right)+2\zeta_\theta\omega_{n_\theta}\left(\Theta(s)-\theta(0^-)\right)+\omega_{n_\theta}^2\Theta(s)=\overline{K}_{\theta\theta}V_{\theta}(s) \qquad \tag{18}θ(Θ(s)s2−sθ(0−)−θ˙(0−))+2ζθ​ωnθ​​(Θ(s)−θ(0−))+ωnθ​2​Θ(s)=Kθθ​Vθ​(s)(18)

Assuming the initial velocity is zero, θ˙(0−)=0\dot{\theta}(0^-)=0θ˙(0−)=0, and solving for position we get

Θ(s)=(s+2ζθωnθ)θ(0−)+K‾θθVθ(s)s2+2ζθωnθs+ωnθ2(19)\Theta(s)=\displaystyle\frac{(s+2\zeta_\theta \omega_{n_\theta})\theta(0^-) +\overline{K}_{\theta\theta}V_{\theta}(s)}{s^2+2\zeta_\theta \omega_{n_\theta}s+\omega^2_{n_\theta}}\qquad \qquad \tag{19}Θ(s)=s2+2ζθ​ωnθ​​s+ωnθ​2​(s+2ζθ​ωnθ​​)θ(0−)+Kθθ​Vθ​(s)​(19)

The pitch step response transfer function matches the standard second-order transfer function in Equation (10). Thus, the damping ratio and natural frequency of the pitch axis can be determined from the pitch step response or alternatively from free response to an initial pitch disturbance.

Yaw Axis: The yaw-only equation of motion by applying a voltage input only to the yaw rotor motor is

ψ¨+1τψψ˙=K‾ψψVψ(t)(20)\ddot{\psi}+ \frac{1}{\tau_\psi} \dot{\psi}= \overline{K}_{\psi\psi}V_\psi(t) \qquad \qquad \tag{20}ψ¨​+τψ​1​ψ˙​=Kψψ​Vψ​(t)(20)

In terms of angular rate, the equation becomes

ω˙ψ(t)+1τψωψ(t)=K‾ψψVψ(t)(21)\dot\omega_\psi(t)+\frac{1}{\tau_\psi}\omega_\psi(t)=\overline{K}_{\psi\psi}V_\psi(t) \qquad \qquad \tag{21}ω˙ψ​(t)+τψ​1​ωψ​(t)=Kψψ​Vψ​(t)(21)

where ωψ(t)=ψ˙(t)\omega_\psi(t)=\dot{\psi}(t)ωψ​(t)=ψ˙​(t) . Taking its Laplace transform

(Ωψ(s)s−ωψ(0−))+1τψΩψ(s)=K‾ψψVψ(s)(22)(\Omega_\psi(s)s-\omega_\psi(0^-))+\frac{1}{\tau_\psi}\Omega_\psi(s)= \overline{K}_{\psi\psi}V_\psi(s)\qquad \qquad \tag{22}(Ωψ​(s)s−ωψ​(0−))+τψ​1​Ωψ​(s)=Kψψ​Vψ​(s)(22)

and solving for the speed we get

Ωψ(s)=ωψ(0−)+K‾ψψVψ(s)s+1τψ=(ωψ(0−)+K‾ψψVψ(s))τψτψs+1(23)\Omega_\psi(s)=\displaystyle \frac{\omega_\psi(0^-)+\overline{K}_{\psi\psi}V_\psi(s)}{ s+\displaystyle \frac{1}{\tau_\psi}} = \displaystyle \frac{(\omega_\psi(0^-)+\overline{K}_{\psi\psi}V_\psi(s))\tau_\psi}{ \tau_\psi s+1} \qquad \qquad \tag{23}Ωψ​(s)=s+τψ​1​ωψ​(0−)+Kψψ​Vψ​(s)​=τψ​s+1(ωψ​(0−)+Kψψ​Vψ​(s))τψ​​(23)

The yaw transfer function matches the first-order transfer function in Equation (7). Thus, the time constant of the yaw axis can be determined from the yaw step response or alternatively from the yaw response to a yaw rate disturbance.

Estimating the Thrust Parameters

A) Estimating K‾θθ\overline{K}_{\theta\theta}Kθθ​

Providing step input only in the pitch axis (applying a voltage to the pitch rotor motor) allows us to focus on the pitch-only system. Plugging Vθ≠0V_\theta \neq 0Vθ​=0 and Vψ=0V_\psi=0Vψ​=0 in Equation (1), the equation of motion for the pitch axis is

θ¨+2ζθωnθθ˙+ωnθ2θ=K‾θθVθ(t)(24)\ddot{\theta} + 2\zeta_\theta\omega_{n_\theta} \dot{\theta} + \omega_{n_\theta}^2 \theta = \overline{K}_{\theta \theta} V_{\theta}(t) \qquad \qquad \tag{24}θ¨+2ζθ​ωnθ​​θ˙+ωnθ​2​θ=Kθθ​Vθ​(t)(24)

Solving for the normalized thrust gain we get

K‾θθ=θ¨+2ζθωnθθ˙+ωnθ2θVθ(t)(25)\overline{K}_{\theta\theta}=\displaystyle \frac{\ddot{\theta} + 2\zeta_\theta\omega_{n_\theta} \dot{\theta} + \omega_{n_\theta}^2 \theta}{V_\theta(t)} \qquad \qquad \tag{25}Kθθ​=Vθ​(t)θ¨+2ζθ​ωnθ​​θ˙+ωnθ​2​θ​(25)

Remark that this is the normalized thrust torque gain parameter. The normalized force thrust gain would be K‾θθ/rp\overline{K}_{\theta\theta}/r_pKθθ​/rp​, where rpr_prp​is the distance between the helicopter pivot and the center of the pitch rotor. Steady State Method: By focusing on the steady-state portion of the system response to a step voltage input to the pitch rotor motor, and neglecting the θ¨\ddot{\theta}θ¨ and θ˙\dot{\theta}θ˙ (zero at steady-state), the normalized thrust gain is obtained as

K‾θθ=ωnθ2θssVθ(26)\overline{K}_{\theta\theta}=\displaystyle \frac{ \omega_{n_\theta}^2 \theta_{ss}}{V_\theta} \qquad \qquad \tag{26}Kθθ​=Vθ​ωnθ​2​θss​​(26)

where VθV_\thetaVθ​ is the magnitude of the voltage input to the pitch rotor motor.

Discrete Derivative Method: The thrust gain parameter can be obtained as

K‾θθ=ΔΩθΔt+2ζθωnθΔΩθVθ(27)\overline{K}_{\theta\theta} =\frac{\frac{\Delta \Omega_\theta}{\Delta t} + 2 \zeta_\theta \omega_{n_\theta} \Delta \Omega_\theta}{V_\theta} \qquad \qquad \tag{27}Kθθ​=Vθ​ΔtΔΩθ​​+2ζθ​ωnθ​​ΔΩθ​​(27)

where Ωθ=θ˙\Omega_\theta = \dot{\theta}Ωθ​=θ˙ and Δt\Delta tΔt is the difference between the time instance when the step input is given tstept_{\text{step}}tstep​ and a time instance slightly above tstept_{\text{step}}tstep​, i.e., t=tstep+t = t_{\text{step}}^+t=tstep+​. Thus we can find the normalized pitch thrust gain from the measured pitch rate and derived acceleration (discrete-derivative of pitch rate).

B) Estimating K‾ψψ\overline{K}_{\psi\psi}Kψψ​

Similarly, to find the normalized thrust gain acting on the yaw axis system, apply a voltage to the tail rotor motor. Plugging Vθ=0V_\theta = 0Vθ​=0 and Vψ≠0V_\psi \neq 0Vψ​=0 in Equation (1), the equation of motion for the yaw axis is

ψ¨+1τψψ˙=K‾ψψVψ(28)\ddot{\psi}+ \frac{1}{\tau_\psi} \dot{\psi}=\overline{K}_{\psi\psi}V_\psi \qquad \qquad \tag{28}ψ¨​+τψ​1​ψ˙​=Kψψ​Vψ​(28)

or,

ω˙ψ+1τψωψ=K‾ψψVψ(29)\dot\omega_\psi+\frac{1}{\tau_\psi} \omega_\psi=\overline{K}_{\psi\psi}V_\psi \qquad \qquad \tag{29}ω˙ψ​+τψ​1​ωψ​=Kψψ​Vψ​(29)

where ωψ=ψ˙\omega_\psi=\dot\psiωψ​=ψ˙​ is the angular rate of the yaw axis. The normalized yaw torque thrust gain is

K‾ψψ=ω˙ψ+1τψωψVψ(30)\overline{K}_{\psi\psi}=\displaystyle\frac{\dot\omega_\psi+\displaystyle \frac{1}{\tau_\psi} \omega_\psi}{V_\psi} \qquad \qquad \tag{30}Kψψ​=Vψ​ω˙ψ​+τψ​1​ωψ​​(30)

Steady State Method: At steady-state condition, and neglecting the ω˙ψ\dot{\omega}_\psiω˙ψ​ (zero at steady-state), the normalized thrust gain is obtained as

K‾ψψ=1τψωψssVψ(31)\overline{K}_{\psi\psi}=\displaystyle\frac{\displaystyle \frac{1}{\tau_\psi} \omega_{\psi_{ss}}}{V_\psi} \qquad \qquad \tag{31}Kψψ​=Vψ​τψ​1​ωψss​​​(31)

where VψV_\psiVψ​is the magnitude of the step voltage input to the yaw rotor motor.

Discrete Derivative Method: The thrust gain parameter can be obtained as

K‾ψψ=ΔΩψΔt+1τψΔΩψVψ(32)\overline{K}_{\psi\psi} =\frac{\frac{\Delta \Omega_\psi}{\Delta t} + \frac{1}{\tau_{\psi}}\Delta \Omega_\psi}{V_\psi}\qquad \qquad \tag{32}Kψψ​=Vψ​ΔtΔΩψ​​+τψ​1​ΔΩψ​​(32)

where Ωψ=ψ˙\Omega_\psi = \dot{\psi}Ωψ​=ψ˙​ and Δt\Delta tΔt is the difference between the time instance when the step input is given tstept_{\text{step}}tstep​ and a time instance slightly above tstept_{\text{step}}tstep​, i.e., t=tstep+t = t_{\text{step}}^+t=tstep+​. Thus we can find the normalized yaw thrust gain from the measured yaw rate and derived acceleration (discrete-derivative of yaw rate).

C) Estimating K‾θψ\overline{K}_{\theta\psi}Kθψ​ and K‾ψθ\overline{K}_{\psi\theta}Kψθ​

The normalized cross-torque thrust parameters, K‾θψ\overline{K}_{\theta\psi}Kθψ​and K‾ψθ\overline{K}_{\psi\theta}Kψθ​ in Equation (3) and (4), represent the coupling between the axes. The cross-torque acting on the pitch axis from a torque applied to the tail rotor motor, can be found by appling a voltage to the tail rotor motor, and examining the response of the pitch. The equations representing these dynamics when Vθ=0V_\theta=0Vθ​=0 and Vψ≠0V_\psi \neq 0Vψ​=0, are

θ¨+2ζθωnθθ˙+ωnθ2θ=K‾θψVψ(33)\ddot{\theta} + 2\zeta_\theta\omega_{n_\theta} \dot{\theta} + \omega_{n_\theta}^2 \theta = \overline{K}_{\theta \psi} V_{\psi} \qquad \qquad \tag{33}θ¨+2ζθ​ωnθ​​θ˙+ωnθ​2​θ=Kθψ​Vψ​(33)

Similarly to identify the cross-torque acting on the yaw axis from a torque applied to the pitch rotor motor, apply a voltage to the pitch rotor motor, and examine the response of the yaw. The equations representing these dynamics when Vθ≠0V_\theta \neq 0Vθ​=0 and Vψ=0V_\psi=0Vψ​=0 , are

ψ¨+1τψψ˙=K‾ψθVθ(34)\ddot{\psi}+ \frac{1}{\tau_\psi} \dot{\psi}=\overline{K}_{\psi\theta}V_\theta \qquad \qquad \tag{34}ψ¨​+τψ​1​ψ˙​=Kψθ​Vθ​(34)

System Identification

Experimental Steps for Finding System Parameters

First download the zip file below and extract in your group folder. The experiment data is automatically generated in the folder MATLAB is opened to.

1) Pitch Motor Voltage Impulse

  1. Unlock the pitch axis. Lock the yaw axis.

  2. Open the pitch_impulse SIMULINK file.

  3. Set the amplitude gain block to 18. This applies a pulse input of -18 V for 1.5 sec in the pitch axis.

  4. Select simulation time 30 sec.

  5. Copy aero_pitch_impulse.mat to your folder.

  6. If the data is not smooth/clean, repeat steps 3-7 with a different pulse voltage.

  7. Close the SIMULINK model. DO NOT SAVE THE CHANGE.

Data is saved in following order: 1: Time 2: Pitch motor input (V) 3: Pitch Angle (rad) 4: Pitch Speed (rad/s)

2) Pitch Motor Voltage Step

  1. Lock the yaw axis.

  2. Open the pitch_step SIMULINK file.

  3. Apply a step input in the pitch axis. If your setup is Aero1, apply 18 V. If your setup is Aero2, apply 14 V.

  4. Select simulation time 100 sec.

  5. Note: Pitch angle has to reach steady state i.e. constant pitch angle, or you may have to increase simulation time. If steady state has reached, you can end simulation earlier.

  6. Copy aero_pitch_step.mat to your folder.

  7. If the data is not smooth/clean, repeat steps 3-7 with a different step voltage.

  8. Close the SIMULINK model. DO NOT SAVE THE CHANGE.

Data is saved in following order: 1: Time 2: Pitch motor input (V) 3: Pitch Angle (rad) 4: Pitch Speed (rad/s)

3) Yaw Motor Voltage Step

  1. Unlock the yaw axis. Lock the pitch axis.

  2. Open the yaw_step SIMULINK file.

  3. Apply a step input in the yaw axis. If your setup is Aero1, pick a voltage in the range 15-20 V. If your setup is Aero2, pick a voltage in the range 10-11.5 V.

  4. Select simulation time 100 sec.

  5. Copy aero_yaw_step.mat to your folder.

  6. If the data is not smooth/clean, repeat steps 3-7 with a different step voltage.

  7. Close the SIMULINK model. DO NOT SAVE THE CHANGE.

Data is saved in following order: 1: Time 2: Yaw motor input (V) 3: Yaw Angle (rad) 4: Yaw Speed (rad/s)

Analysis

  1. From the pitch response due to pitch motor impulse input, find the natural frequency and damping ratio

    • You can also use the above-attached Aero_Pitch_SystemID.mlx script to find ωn\omega_nωn​ and ζ\zetaζ . Complete the lines marked with TODO.

  2. From the pitch response due to pitch motor step input find the normalized thrust gain K‾θθ\overline{K}_{\theta\theta}Kθθ​, using both the steady state method and discrete derivative method.

    • You can also use the above-attached Aero_Pitch_SystemID.mlx script to find K‾θθ\overline{K}_{\theta\theta}Kθθ​. Complete the lines marked with TODO.

  3. From the yaw speed response due to yaw motor step input find the time constant and normalized thrust gain K‾ψψ\overline{K}_{\psi\psi}Kψψ​

    • You can use the above-attached Aero_Yaw_SystemID.mlx script to find τ\tauτ and K‾ψψ\overline{K}_{\psi\psi}Kψψ​. Complete the lines marked with TODO.

Table B.1

System ID Parameters
Value

Table B.2

Thrust Gain Parameters
Steady State Method
Discrete Derivative Method

Results for Report

  1. Equations used for calculating ωn\omega_nωn​, ζ\zetaζ, τ\tauτ, K‾θθ,K‾ψψ\overline{K}_{\theta\theta},\overline{K}_{\psi\psi}Kθθ​,Kψψ​.

  2. Table B.1 and Table B.2.

Questions for Report

  1. Compare the normalized thrust gains obtained from the steady-state method and discrete derivative method and explain any differences.

Estimating K‾θψ\overline{K}_{\theta\psi}Kθψ​ and K‾ψθ\overline{K}_{\psi\theta}Kψθ​ are out of scope of this lab. The values of these parameters are provided in the section.

To build the model, click the down arrow on Monitor & Tune under the Hardware tab and then click Build for monitoring . This generates the controller code.

Click Connect button under Monitor & Tune and then run SIMULINK by clicking Start .

To build the model, click the down arrow on Monitor & Tune under the Hardware tab and then click Build for monitoring . This generates the controller code.

Click Connect button under Monitor & Tune and then run SIMULINK by clicking Start .

To build the model, click the down arrow on Monitor & Tune under the Hardware tab and then click Build for monitoring . This generates the controller code.

Click Connect button under Monitor & Tune and then run SIMULINK by clicking Start . Note: Pitch angle has to reach steady state i.e. constant pitch angle, or you may have to increase simulation time. If steady state has reached, you can end simulation earlier.

The cross-torque thrust parameters are K‾θψ=0.0959\overline{K}_{\theta\psi} = 0.0959Kθψ​=0.0959 Nm/V, K‾ψθ=−0.1227\overline{K}_{\psi\theta} = -0.1227Kψθ​=−0.1227 Nm/V. These will be required for .

Hint: Use equations

Fill the corresponding values in

Hint: For the steady-state method, use equation and for the discrete derivative method, use equation .

Fill in the values in .

Hint: For time constant. use equation . For K‾ψψ\overline{K}_{\psi\psi}Kψψ​, use equation for the steady-state method and use equation for the discrete derivative method.

Fill the respective values in and .

The given value of K‾θψ\overline{K}_{\theta\psi}Kθψ​ is positive and K‾ψθ\overline{K}_{\psi\theta}Kψθ​ is negative for our setup (see Step 1 of section). Why?

ωnθ\omega_{n_\theta}ωnθ​​
ζθ\zeta_\thetaζθ​
τψ\tau_\psiτψ​
K‾θθ\overline{K}_{\theta\theta}Kθθ​
K‾ψψ\overline{K}_{\psi\psi}Kψψ​
Controller Design
Analysis
12-16
Table B.1
26
27
Table B.2
8
31
32
Table B.
1
Table B.2
Analysis
111KB
Aero_Lab2.zip
archive
55KB
Aero_Pitch_SystemID.mlx
MATLAB script to find ωn\omega_nωn​,ζ\zetaζ and K‾θθ\overline{K}_{\theta\theta}Kθθ​
47KB
Aero_Yaw_SystemID.mlx
MATLAB script to find τ\tauτ and K‾ψψ\overline{K}_{\psi\psi}Kψψ​
Figure 1b: Pendulum effect creating pitch stiffness due to vertical offset of cg from pitch pivot.
Pitch impulse response
Pitch step response
Yaw step response for Aero1 setup.
Fig. 1a: Quanser Aero Experiment
Fig. 2: Simple free-body diagram of Quanser Aero Experiment
Fig. 3 : First-order step response
Fig. 4: Step Response of an underdamped second order system.