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 two types of controllers, viz., Servo Controller and Link Controller. We will compare the closed loop performance between the two controllers.

• In the Servo Controller design, partial state feedback is used, i.e., only the servo angle $({\theta})$ and its rate ($\dot{\theta}$) are used as feedback variables. We will make use of the controlSystemDesigner tool in MATLAB with simultaneous loop closure architecture for the controller.

• In the Link Controller design, full state feedback is used, i.e, the servo angle $({\theta})$ and its rate $(\dot{\theta})$, as well as the link angle $({\alpha})$ and its rate $(\dot{\alpha})$, are used as feedback variables. 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 state 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 matrix $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}_{max}^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 rise time: $t_r≤0.2s$

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.

State Space Model

639B

A_and_B_matrices.m

Open

1. Download A_and_B_matrices.m file and input your $K_{s}$ value from System Identification.

2. 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.

Servo Controller Design (PD Controller):

1. For a rapid rotation of the link, we select the proportional gain based on using the maximum voltage input at the beginning. For a 30 degrees $( = \frac{\pi}{6} rad)$ of step command, required proportional gain $= \frac{V_{max}}{\theta_{command}} = \frac{10}{\frac{\pi}{6}}$ = 19.1. Select Proportional gain $(K_{\theta})$ = 19.

2. With the servo angle $({\theta})$ as output, construct the state space model in MATLAB. >> sys_servo = ss(A,B,[1,0,0,0],0)

3. Open the controlSystemDesigner tool in MATLAB. >> controlSystemDesigner(sys_servo)

4. Under Edit Architecture, select the controller structure shown in the figure. Note that C1 is the proportional gain block and C2 is the derivative gain block. Hence, you need to add a differentiator in C2, not a real zero.

1. Set the gain value in C1 to 19 (see step 1).

2. Use the step response plot with the rise time and peak response characteristics selected, and adjust the derivative gain C2 to meet the given specs for the rise time and the percentage overshoot. The value of C2 is the feedback gain for the servo angle rate, i.e., $(K_{\dot{\theta}})$. Check your gain values with the TA.

Link Controller Design:

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

$θ_{max, initial} = 0.05 rad, α_{max}=0.1 rad,$ $\dot{θ}_{max}=1rad/s, \dot{α}_{max}=0.5rad/s$ $V_{max}=1 V$ Check if $Q \ge 0$ and $R > 0$ (by finding eigenvalues of the matrix).

1. 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.

1. Check if the gain associated with servo angle $({\theta})$ feedback matches with its selected value in the servo controller design, i.e., $K_{\theta}$ = $K(1,1)$ = 19. If not, adjust the weight $q_{\theta}$ (increase the weight to increase the gain and decrease the weight to decrease the gain) while keeping the rest of the weights same. Repeat the lqr command till $K_{\theta}$ = $K(1,1)$ = 19. Check your gain matrix K with the TA.

Simulation Evaluation:

1. 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)

2. Using the controller gains found in step 3 of Link Controller Design, 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$.

1. Using the controller gains found in step 6 of Servo Controller Design, simulate the closed loop system for a step servo position command of $30\degree$over a 10s duration.

   • This is easily done by selecting the gain matrix K used in your Simulink model as $K_{\theta}$ and $K_{\dot{\theta}}$ values from step 6 of Servo Controller Design and $K_{\alpha}$=0, and $K_{\dot{\alpha}}$=0. Check your plots with the TA.

   • 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 Link Controller found. Measurements will then be taken to ensure that the specifications are satisfied. Also, we will implement the Servo Controller found for comparison.

32KB

q_flex.zip

archive

Open

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 Link 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 using the gains obtained from your Servo Controller Design. 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 A and B matrices.

2. Include the results from the Servo Controller Design procedure (steps 1 through 6).

3. Include the results from the Link Controller Design procedure (steps 1 through 3).

Questions

1. Estimate the bandwidth for your Link and Servo 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 Link and Servo controllers.

1. Estimate the stability margins (gain and phase margins) of your Link and Servo 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 Link and Servo controllers.

Controller Implementation

Results

1. Run the Simulink that you built in control design with the same command input used in the experiment. Note: Average the first few seconds of the experimental data to obtain initial conditions for your state-space block in Simulink.

2. Plots of servo angle, link deflection angle, and input voltage vs time for Link Controller and Servo Controller. On the same plot, show the experimental results and simulation results with the Link Controller and the Servo Controller. Clearly label your plots.

3. Compare the experimental results using the two controllers and explain the differences.

4. Compare the experimental results with the simulation results individually for each controller and explain the differences.

Questions

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

2. Explain the impact of reducing the control weight R on the gain matrix K.