B & C. Control Design and Controller Implementation (Week 2)

Objective

The objective is to design a controller that allows rapid reorientation of the link with very little vibrations from the link flexibility. This analysis is a preliminary approach to containing structural vibrations in real-world applications like aircraft and spacecraft structures as well as robotic links/manipulators

B. Control Design

The methodology used is to design the control law assuming full state feedback to meet a given set of specifications. This will be followed by studying the loss of controller performance with servo feedback alone. The method we will use for full state feedback is based on the linear quadratic regulator (LQR) theory, which requires the given system to be controllable.

State Space Model

The equations that describe the motions of the servo and the link with respect to the servo motor voltage are obtained using the lumped mass method and can be written as

$$(J_{eq} + J_l)\ddot{\theta} + J_l\ddot{\alpha} + B_{eq}\dot{\theta} = \tau \quad \quad \tag{1}$$

$$J_{l}\ddot{\theta} + J_l\ddot{\alpha} + B_{l}\dot{\theta} + K_s\alpha= 0 \quad \quad \tag{2}$$

Assuming the viscous damping of the link is negligible, i.e. $B_l$ = 0 and substituting the torque $\tau$ using the expression,

$$\tau = \displaystyle\frac{\eta_gK_g\eta_mk_t(V_m - K_gk_m\dot\theta)}{R_m} = C_1V_m - C_2\dot\theta$$

the EOM can be rearranged as

$$\ddot{\theta} = -(\frac{B_{eq} + C_2}{J_eq})\dot{\theta} + \frac{K_s}{J_{eq}}\alpha + \frac{C_1}{J_{eq}}V_m \quad \quad \tag{3}$$

$${\ddot{\alpha}} = (\frac{B_{eq}+C_2}{J_{eq}}){\dot{\theta}} - K_s(\frac{J_{eq}+J_l}{J_{eq}J_l}){\alpha }- \frac{C_1}{J_{eq}}V_m \quad \quad \tag{4}$$

The various system parameters can be found in Table 2 of Part A: System Identification.

Equations 3 and 4 can be represented in state-space form

$$\dot{x} = Ax + Bu \\ y = Cx + Du$$

with$x=[\theta \;\; \alpha \;\; \dot{\theta} \;\; \dot{\alpha}]^T$ and$u = V_m$, where

$$A = \begin{bmatrix} 0 &0 & 1 &0 \\ 0 &0 & 0 &1 \\ 0 &\frac{K_s}{J_{eq}} & -(\frac{B_{eq}+C_2}{J_{eq}}) &0 \\ 0 &-K_s(\frac{J_{eq}+J_l}{J_{eq}J_l}) & (\frac{B_{eq}+C_2}{J_{eq}}) & 0 \end{bmatrix} \quad \quad \tag{5}$$

$$B = \begin{bmatrix} 0 \\ 0\\ \frac{C_1}{J_{eq}} \\ -\frac{C_1}{J_{eq}} \end{bmatrix} \quad \quad \tag{6}$$

In the output equation, only the position of the servo and link angles are being measured.

As such, the velocities of the servo and link angles can be computed in the digital controller if required, e.g., by taking the derivative and filtering the result though a high-pass filter. Hence, with all the states of the system as outputs, the C and D matrices are

$$C = \begin{bmatrix} 1 & 0& 0& 0\\ 0 & 1& 0& 0\\ 0 & 0& 1& 0\\ 0 & 0& 0& 1\ \end{bmatrix} \qquad \qquad \tag{7}$$

$$D = \begin{bmatrix} 0\\ 0 \\0 \\ 0 \end{bmatrix} \qquad \qquad \tag{8}$$

Linear Quadratic Regulator (LQR) Design

Controllability

If the control input $'u'$ of a system can take each state variable, $x_i$ where $i = 1 . . . n$, from an initial state to a final state in finite time, then the system is controllable, otherwise it is uncontrollable. The controllability of a system can be determined using the ‘Rank Test’. According to the Rank Test, the system is controllable if the rank of its controllability matrix

$$T=[B \;\; AB \;\;A^2B… A^{n−1}B]$$

equals the number of states of the system,

$$rank(T)=n$$

LQR controller

If (A,B) are controllable, then the Linear Quadratic Regular optimization method can be used to find full state feedback controller gains. Given the plant model in state-space form where $x$ represents the state vector, this technique finds a control input ‘$u$’ that minimizes the cost function

$$J= \int_0^\infty(x^TQx+ u^TRu)dt \qquad \qquad \tag{9}$$

where Q and R are the user-defined weighting matrices $Q = Q^T$ and $R = R^T$. It is assumed that Q is positive semi-definite (all the eigenvalues of$Q$ are non-negative) and $R$ is positive definite (all the eigenvalues of$R$ are positive). The weighting matrices affect how LQR minimizes the function and are, essentially, tuning variables. Here,$x$ is the state vector$[\theta \;\; \alpha \;\; \dot{\theta} \;\; \dot{\alpha}]^T$ and$u$ is the servo motor voltage input $V_m$.

Given the control law $u = −Kx$, the state-space model of the closed loop system becomes

$$\dot{x} = Ax+B(−Kx)=(A−BK)x \quad \quad \tag{10}$$

where $K$ is given by

$$K=[k_{\theta} \;\;k_{\alpha}\;\;k_{\dot{\theta}}\;\;k_{\dot{\alpha}}] \qquad \qquad \tag{11}$$

is the controller gain vector. Note that the input voltage $Vm$ is scalar, and therefore, $R$ is also scalar. In practice, the weighting matrices $Q$ and $R$ are chosen to be diagonal. A simple way for choosing $Q$ and $R$ is using the Bryson's rule. This method works best when control design specifications regarding the system response and control input are given in terms of limits on excursions from equilibrium.

Assuming the time-domain design specifications are

$$|x_i| \leq x_{i_{\max}} \; i = 1, 2, ..., n \; \; \; \\ |u_j| \leq u_{j_{\max}} \; j = 1, 2, ..., m$$

Then one can use the Bryson’s rule to choose the weighting matrices as

$$Q=\displaystyle {diag\left (\frac{1}{x_{i_{\max}}^2}\right) \; \; i = 1, 2, ..., n} \quad \quad (12) \\ R = diag\left (\frac{1}{u_{j_{\max}}^2}\right) \; \; j = 1, 2, ..., m \quad \quad (13)$$

Further tuning of $Q$ and $R$ may be required for improving the controller performance. Note that increasing $R$ means increasing the penalty on the control input (using less control effort). Similarly, increasing $Q$ means increasing the penalty on the states (states reaching equilibrium position quickly and with less overshoot). Matrix $Q$ sets the weight on the states and determines how $u$ will minimize $J$ (and hence how it generates gain $K$).

In our case,

$$Q = \begin{bmatrix} \frac{1}{\theta_{max}^2} & 0& 0& 0\\ 0 & \frac{1}{\alpha_{max}^2}& 0& 0\\ 0 & 0& \frac{1}{\dot{\theta}_{max}^2}& 0\\ 0 & 0& 0& \frac{1}{\dot{\alpha}_{max}^2}\ \end{bmatrix} \qquad \qquad \tag{14}$$

$$R = \frac{1}{V^2_{m, max}} \quad \quad (15)$$

Substituting $Q$ and $R$ in Eqn. 9 gives

$$J= ∫_0^\infty (q_{θ}\theta^2+q_{α}\alpha^2+ q_{\dot{θ}}{\dot{\theta}}^2+q_{\dot{α}}{\dot{\alpha}}^2+RV_m^2)dt \quad \quad \tag{16}$$

where $q_\theta= \displaystyle\frac{1}{\theta_{max}^2}$, $q_\alpha= \displaystyle\frac{1}{\alpha_{max}^2}$, $q_{\dot{\theta}}= \displaystyle\frac{1}{\dot{\theta}_{max}^2}$, $q_{\dot{\alpha}}= \displaystyle\frac{1}{\dot{\alpha}^2}$, $R=\displaystyle\frac{1}{V_{m,max}^2}$

On observing Eq. 12, it can be seen that increasing $q_i$ causes control input $V_m$ to work harder to minimize state $x_i$, which will predominately increase$k_i$. If $q_{\theta}$ is increased, then $k_{\theta}$ will increase to compensate for the larger weight placed on state $\theta$. Depending on the model, changing a single$q_i$ can affect multiple$k_i$ gains because each state is not independent. If$R$ is increased, then control input $V$ has to work less to minimize$J$. In that case, LQR would generate a lower overall value of$K$. A block diagram of the system with fill state feedback controller is shown in Figure 1.

Figure 1. State-feedback Control Loop (Note:$C$ is$4×4$ identity matrix and$D$ is$4×1$ column of zeros.)

The feedback control loop in Figure 1 is designed to stabilize the servo to a desired position,$θ_d$, while minimizing the deflection of the flexible link, where$x$ is the state column vector and$K$ is the gain row vector as shown earlier. Note that$C$ is$4×4$identity matrix and$D$ is $4×1$ column of zeros in Figure 1.

The reference state is defined as

$$x_d=[θ_d \;\; 0\;\; 0\;\;0]^T$$

and the controller is

$$u=K(x_d−x)$$

Controller Specifications:

The following specifications in the time domain are to be met when the rotary arm is tracking a $30°$step change in its angular position.

Specification 1: Servo angle 2% settling time: $t_s≤0.5s$

Specification 2: Servo angle percentage overshoot: $PO≤5 \%$

Specification 3: Maximum link angle deflection:$|α|_{max}≤10°$

Specification 4: Maximum control effort (voltage): $|V_{m}|_{max}≤10 V$

Controller Design Procedure:

Note: The results from the following steps have to be included in the Analysis section of the report.

1. Obtain the $A$ and $B$ matrices of the system using the parameters from Part A: System identification. Check your A and B matrices with the TA.

2. Using Bryson’s rule (Eq. 12-15) calculate $Q$ and $R$ matrices, based on the following suggested maximum values of state and control excursions:

   $0.05 rad ≤|θ_{max}|≤0.1 rad \;\;\;\;\;\; |α_{max}|\simeq0.1 rad$ $∣\dot{θ}_{max}∣\simeq1rad/s \;\;\;\; \; \; 0.4rad/s≤|\dot{α}_{max}|≤0.6rad/s$ $0.8 V ≤ |V_{m,max}|≤1.2 V$ Check if $Q \ge 0$ and $R > 0$ (by finding eigenvalues of the matrix).

3. Using the lqr command in MATLAB, calculate the optimal feedback gain vector $K$. The lqr command solves the Algebraic Riccati equation which is required to determine the gain vector $K$ is

   >> [K,S,E] = lqr(A,B,Q,R);

   where $K$ is the gain matrix, $S$ is the solution to the Riccati equation and E are closed loop system poles. Check your gain matrix K with the TA.

4. Using Figure 1 as a guide, develop a Simulink model of the system.

   Note: SIMULINK references the current MATLAB workspace when setting variables. Use the variable names for the system matrices $A$ and $B$ and the control gain matrix $K$ from your MATLAB script file for the LQR design.

   Make sure to include servo position command input, 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)

5. Using the controller gains found in step 3, simulate the closed loop system for a step servo position command of $30\degree$over a 10s duration.

   • Record (and plot for lab report) the servo angle ($\theta$) link angle ($\alpha$) and voltage input $V_m$.

   • Check if all the given specifications are met.

   • If not, keep adjusting the $Q$ and $R$ weighting matrices parameters, till all specifications are met. Record the final Q and R matrices, the servo angle ($\theta$), link angle ($\alpha$) and voltage input $V_m$plots.

6. Study in simulation the degradation, if any, in closed loop system performance (in meeting the given specifications) using partial feedback. For partial feedback consider feedback of servo angle ($\theta$) and servo angle rate ($\dot{\theta}$).

   • This is easily done by retaining the gain value of the chosen feedback loops and zeroing out the remaining gains in the gain matrix $K$ used in your Simulink model.

   • Check your plots with the TA.

   • Check if the specifications are met (no need to tune Q and R matrices.)

   • Record (and plot for lab report) the servo angle ($\theta$) link angle ($\alpha$) and voltage input $V_m$

C. Controller Implementation and Evaluation

In this experiment, the servo position is controlled while minimizing the link deflection using the LQR­ based control found. Measurements will then be taken to ensure that the specifications are satisfied.

32KB

q_flex.zip

archive

Warning: In closed loop experiments, the link may swing around rather quickly. Stay clear of it during closed loop experiments.

Experiment

1. Turn the amplifier power on.

2. Download the q_flex.zip and open the Simulink file q_flex for controller implementation. This is the block diagram for this part of the experiment. Make sure you have K gain variable in the MATLAB workspace. Note: The Smooth Signal Generator block generates a 0.33 Hz square wave (with amplitude of 1) that is passed through a Rate Limiter block to smooth the signal (Rising slew rate of 100 and falling slew rate of -100). The Amplitude (deg) gain block is used to change the desired servo position command. The state-feedback gain K is set in the LQR Control gain block and is read from the MATLAB workspace.

   
3. To build the model, click down arrow on Monitor & Tune under Hardware tab and then Build for monitoring . This generates the controller code.
4. Align the flexible link along the 0 mark on the servo stand.

5. Double click on the scopes and open them.

6. Ensure that the manual switch is connected to the full state feedback.

7. Click Connect button under Monitor & Tune and then click Start
8. A periodic square wave is given as commanded link rotation to the servo motor, causing the link to move left and right alternately for 10 seconds.

9. Copy the response data to your group folder. The response data is saved in variables data_theta, data_alpha, and data_vm. DO NOT DELETE or SAVE THE SIMULINK MODEL.

10. Connect the manual switch to the partial state feedback and repeat steps 4-9 again. Make sure your files name is different from full state feedback.

11. Turn the power off.

Results & Questions for Report

Control Design

Results

1. Include the results from the controller design procedure Steps 1-6.

Questions

1. Estimate the bandwidth for your full state and partial state feedback controllers. Hint: Use 'bandwidth(sysclosed)' command in MATLAB with link rotation ($\theta$) as the output.

>> sysclosed = ss((A-B*K), B, [1 0 0 0], 0);

>> bw = bandwidth(sysclosed)

>> bode(sysclosed)

The bode() function produces closed loop system frequency response plot. Include Bode Plots of the closed loop system and list the bandwidth of the system with full and partial state feedback.

1. Estimate the stability margins (gain and phase margins) of your full state and partial state feedback controllers. Hint: Use 'margin(sysopen)' command in MATLAB where 'sysopen' is the state space model of the open loop system with the controller.

>> sysopen = ss(A,B,K,0)

>> margin(sysopen)

We get bode plot of the open loop system with the controller with margins listed on the plots. Include the Bode plots for the full state and partial state feedback.

Controller Implementation

Results

1. Plots of servo angle, link deflection angle and input voltage vs time for full state and partial state feedback. Compare the results and explain the differences.

2. Run the Simulink that you built in control design with the same command input used in the experiment. Compare the simulated response with the experimental response for both full state and partial state feedback and explain any differences between them. Note 1: Generate two sets of figures with one set containing the plots of simulation and experiment for full state feedback and the second set containing the plots of simulation and experiment for partial state feedback. For each feedback case, the simulated response and corresponding experimental response must be on the same graph. Note 2: Average the first few seconds of the experimental data to obtain initial conditions for your state-space block in Simulink.

Questions

1. Explain why the link deflection angle goes opposite to servo rotation initially.

2. For a controllable system, can partial state feedback guarantee stability like full state feedback? Briefly explain.