B. Control Design

Objective

The objective of this experiment is to design a position control system for the Quanser Aero system in the 2 DOF configuration. The angular position controller should meet the following specifications:

Design Specifications

  1. Steady-state error to a step command input: (a) ess,pitche_{ss, pitch} = 0 deg (b) ess,yawe_{ss, yaw} = 0 deg

  2. Peak time of response to a step command input: (a) tp,pitcht_{p, pitch}≤ 2s (b) tp,yawt_{p, yaw}≤ 2s

  3. Percent Overshoot of response to a step command input: (a) POpitchPO_{pitch}≤ 7.5% (b) POyawPO_{yaw} ≤ 7.5%

  4. No actuator saturation: (a) Vθ|V_\theta|≤ 24V (b) Vψ|V_\psi|≤ 24V

Refer to the lecture note on control design.

Download Matlab Script

Download the setup aero file below and fill in the system parameters, control parameters and other necessary values to store in the MATLAB workspace. Use these variables in building Simulink so it is easier for you to put values. Make sure to check your values with TAs before running Simulink.

Controller Design

We will start our controller design by considering first the decoupled system. In this case, the Multiple-Input Multiple-Output (MIMO) system is represented as two separate Single-Input Single-Output (SISO) systems.

The pitch dynamics of the 2D Aero system is a Type 0 system, i.e., no pole at the origin. As such, we will select the PID controller structure for the pitch control design, which will meet the specification of zero steady-state error to a step command input. The yaw dynamics of the 2D Aero system is a Type 1 system, i.e., there is one pole at the origin. As such, we will select the PD controller structure for the yaw control design, which will meet the specification of zero steady-state error to a step command input.

  1. Choose appropriate values for POPO and tpt_p in both pitch and yaw axes satisfying the design specifications.

  2. For these selected values of POPO and tpt_p within the design specs, compute the desired values of damping ratio ζ\zeta and natural frequency ωn\omega_n using the following equations: PO=(eζπ1ζ2)100%PO = \left(e^{ \frac{-\zeta \pi}{\sqrt{1 - \zeta^2}}}\right)*100\% tp=πωdt_p = \displaystyle \frac{\pi}{\omega_d} ωd=ωn1ζ2\omega_d = \omega_n \sqrt{1 - \zeta^2}

  3. Using the aforementioned desired values of ζ\zeta and ωn\omega_n and the system parameters calculated in part A, compute the proportional gain KpK_p and the derivative gain KdK_d individually for pitch and yaw controllers as follows: Note: Let subscript op\text{op} represent open-loop (actual system parameters), and subscript cp\text{cp} represent closed-loop (desired parameters). Pitch: Kp=ωncl2ωnop2KθθK_p = \displaystyle \frac{\omega_{n_\text{cl}}^2 - \omega_{n_\text{op}}^2}{\overline{K}_{\theta \theta}} Kd=2ζclωncl2ζopωnopKθθK_d = \displaystyle \frac{2 \zeta_{\text{cl}} \omega_{n_\text{cl}} - 2 \zeta_{\text{op}} \omega_{n_\text{op}}}{\overline{K}_{\theta \theta}} Yaw: Kp=ωncl2KψψK_p = \displaystyle \frac{\omega_{n_\text{cl}}^2}{\overline{K}_{\psi \psi}} Kd=2ζclωncl1τopKψψK_d = \displaystyle \frac{2 \zeta_{\text{cl}} \omega_{n_\text{cl}} - \displaystyle \frac{1}{\tau_{\text{op}}}}{\overline{K}_{\psi \psi}} Note: In the above calculations, use the normalized thrust gains obtained using the steady-state method.

  4. Add an integral term, KiK_i, to the pitch controller. Choose KiK_i to be a value in the range 1%10%1\%-10\% of KpK_p. Caution: Integral feedback, while improving steady-state response, introduces lag. Large values of integral gain, while reducing damping, can lead to loss of stability of the closed loop system.

  5. Initially, set the feedforward gains (KθtoψK_{\theta-to-\psi} and KψtoθK_{\psi-to-\theta}) to 0, i.e., ff_py = 0 and ff_yp = 0 in the MATLAB code. Eventually, you will choose appropriate values for the feedforward gains such that the coupling effect is minimized. For example, choose KθtoψK_{\theta-to-\psi} in Figure 1, to cancel the torque from the pitch rotor, i.e. , Kθtoψ=KψθKψψ(1)K_{\theta-to-\psi} = \displaystyle - \frac{\overline{K}_{\psi\theta}}{\overline{K}_{\psi\psi}} \qquad \qquad (1) Likewise, choose KψtoθK_{\psi-to-\theta} to cancel the torque from the yaw rotor, i.e. , Kψtoθ=KθψKθθ(2)K_{\psi-to-\theta} = \displaystyle - \frac{\overline{K}_{\theta\psi}}{\overline{K}_{\theta\theta}} \qquad \qquad (2) Feedforward controller minimizes the coupling effects. Use Figure 1 as a reference for how to add the two feedforward paths.

Figure 1: Feedforward controller architecture

Simulation Modelling and Evaluation

  1. Create a SIMULINK model of the system. (a) Create subsystem blocks representing the plant. Figure 2 shows separate pitch and yaw plants. Note: Plants account for coupling (see Figures 3(a) and 3(b)). (b) Create subsystem blocks representing the decoupled pitch and yaw controllers. Figure 2 shows the input/output structure of these blocks. (c) Using the completed subsystem blocks, finish the diagram of the closed-loop system. Make sure to include pitch and yaw inputs, saturation blocks, and appropriate outputs. Use scopes and To Workspace blocks so that reference, output, and response signals can be tracked. (Make sure to maintain consistency between deg and rad in SIMULINK) (d) Set the sampling time to Fixed-step of step-size 0.002s in Modeling -> Model Settings.

Figure 2: Controller, saturation, and Plant blocks from the SIMULINK diagram.
Figure 3(a): Pitch plant Simulink block
Figure 3(b): Yaw plant Simulink block

Decoupled Dynamics with Decoupled Controller

First consider simulation validation of your controller using decoupled dynamics and decoupled controller (i.e., no feedforward between pitch and yaw channels). 1. Set the values of Kθψ=0\overline{K}_{\theta\psi} = 0 in the pitch plant Simulink block and Kψθ=0\overline{K}_{\psi\theta} = 0 in the yaw plant Simulink block. This will remove the coupling effects between pitch and yaw dynamics. 2. Set the integral gain of the pitch controller Ki=0K_i = 0. 3. Simulate the closed loop system response with yaw-only command: θd=0°\theta_d = 0 \degreeand ψd=30°\psi_d = 30 \degree. Save the data. 4. Check whether your controller meets the desired specifications for the yaw command. Since decoupled system dynamics are used in this step, you will likely see a close match between your simulation results and the desired specifications. 5. Set the integral gain of the pitch controller Ki1%10%K_i \approx 1\% - 10\% of KpK_p (refer to Step 4 of Controller Design). 6. Simulate the closed loop system response with pitch-only command: θd=10°\theta_d = 10 \degreeand ψd=0°\psi_d = 0 \degree. Save the data. 7. Check if your pitch controller meets all the desired specifications. If it fails to meet the POPO and tpt_p specifications, relax the specifications and/or tune the pitch PID controller gains to try to meet the specifications. Save the tuned results.

Coupled Dynamics without Feedforward Controller

Next, consider the simulation evaluation of the feedback controller for the system with coupled dynamics. 1. Set the values of Kθψ\overline{K}_{\theta\psi} in the pitch plant Simulink block and Kψθ\overline{K}_{\psi\theta} in the yaw plant Simulink block to the estimated values from Part A: System Identification. This accounts for the coupled dynamics. 2. Simulate the closed-loop response of the system with pitch-only command: θd=10°\theta_d = 10 \degreeand ψd=0°\psi_d = 0 \degree. Save the data. 3. Simulate the closed-loop response of the system with yaw-only command: θd=0°\theta_d = 0 \degreeand ψd=30°\psi_d = 30 \degree. Save the data. 4. Simulate the closed-loop response of the system with simultaneous pitch and yaw commands: θd=10°\theta_d = 10 \degreeand ψd=30°\psi_d = 30 \degree. Save the data.

Coupled Dynamics with Feedforward Controller

  1. Set the feedforward gains KθtoψK_{\theta-to-\psi} and KψtoθK_{\psi-to-\theta} based on Eqs. 1 and 2 (marked as ff_py and ff_yp in the MATLAB code). These gains are different from the normalized cross-thrust torque gains that you set to make the coupled dynamics.

  2. Simulate the closed-loop response of the system with pitch-only command: θd=10°\theta_d = 10 \degreeand ψd=0°\psi_d = 0 \degree. Save the data.

  3. Simulate the closed-loop response of the system with yaw-only command: θd=0°\theta_d = 0 \degreeand ψd=30°\psi_d = 30 \degree. Save the data.

  4. Simulate the closed-loop response of the system with simultaneous pitch and yaw commands: θd=10°\theta_d = 10 \degreeand ψd=30°\psi_d = 30 \degree. Save the data.

Results for Report

(A) Simulation Modelling

Include a screenshot of the complete SIMULINK model (block diagram). In addition, include the following subsystem block diagram screenshots:

  • Pitch and Yaw plants

  • Pitch and Yaw controllers

(B) Simulation Evaluation

  1. Mention the chosen design specifications values (Step 1 of Controller Design).

  2. Mention the final yaw PD gains (Step 3).

  3. Yaw-only command case: State if the design specifications were met, either including figures with design specs marked (Step 3) OR including a table with the POψPO_{\psi}, tpψt_{p_\psi}, essψe_{ss_\psi} and maxVψ\max |V_\psi|values recorded from the response (Step 4).

  4. Mention the final tuned pitch PID gains (Step 7).

  5. Pitch-only command case: State if the design specifications were met, either including figures with design specs marked (Step 7) OR including a table with the POθPO_{\theta}, tpθt_{p_\theta}, essθe_{ss_\theta} and maxVθ\max |V_\theta|values recorded from the response (Step 7).

  1. Pitch-only command case: Plot the pitch and yaw data (commanded angles, simulation responses and motor input voltages) for pitch-only command (Step 2) and check if the design specifications were met in pitch.

  2. Yaw-only command case: Plot the pitch and yaw data (commanded angles, simulation responses and motor input voltages) for yaw-only command (Step 3) and check if the design specifications were met in yaw.

  3. Pitch and yaw combined case: State if the design specifications were met, either including figures with design specs marked (Step 4) OR including a table with the POθPO_{\theta}, tpθt_{p_\theta}, essθe_{ss_\theta}, maxVθ\max |V_\theta|, POψPO_{\psi}, tpψt_{p_\psi}, essψe_{ss_\psi} and maxVψ\max |V_\psi|values recorded from the responses (Step 4).

Note: It is possible that specifications are not met due to coupling between pitch and yaw dynamics.

  1. Mention the feedforward gain values.

  2. Pitch-only command case: Plot the pitch and yaw data (commanded angles, simulation responses and motor input voltages) for pitch-only command (Step 2) and check if the design specifications were met in pitch.

  3. Yaw-only command case: Plot the pitch and yaw data (commanded angles, simulation responses and motor input voltages) for yaw-only command (Step 3) and check if the design specifications were met in yaw.

  4. Pitch and yaw combined case: State if the design specifications were met, either including figures with design specs marked (Step 4) OR including a table with the POθPO_{\theta}, tpθt_{p_\theta}, essθe_{ss_\theta}, maxVθ\max |V_\theta|, POψPO_{\psi}, tpψt_{p_\psi}, essψe_{ss_\psi} and maxVψ\max |V_\psi|values recorded from the responses (Step 4).

Questions for Report

  1. How does accounting for the coupling affect the performance of the controller? (a) What happens when you neglect the coupling effect (in simulation)? (b) What is a physical explanation for the coupling effect?

  2. Why a PID controller is used for the pitch and a PD controller for the yaw?

  3. Estimate gain and phase margins of the closed loop systems individually with your pitch and yaw controllers. Hint: Use the margin command in MATLAB. Gain and phase margins

Last updated

Was this helpful?