ME 417 Control of Mechanical Systems

Numerical Lesson #2

PART I: TIME RESPONSE ANALYSIS

A. Disk Drive Time Response Analysis

A disk drive read head can be modeled as a pair of rotating masses connected by a flexible shaft. As shown on Figure 1. $I_{m}$ represents the motor's inertia, while $I_{h}$ represents the head's inertia. The flexible shaft has a stiffness $K$ and damping coefficient $b$ . $M_{m}$ is the torque applied by the motor while $M_{D}$ is the disturbance torque.

Figure 1 - Disk Drive Model

EXERCISE 1

Using the impedance method, find the equations of motion for the system in the Laplace Domain

For the following questions, use the parameters from Table 1

Derive the transfer function $G_{2}( s ) = \dfrac{s^2\Theta_h} {M_m} = \dfrac{\ddot{\theta}_h(s)}{M_{m}(s)}$ symbolically.

Hint: Solve for a linear systems of equation or use Cramer's rule.

Simulate the response to a step input using the ControlSystems.jl stepplot() command.

Table 1 Disk Drive Parameters

Parameter	$I_{m}$	$I_{h}$	$K$	$M_{D}(t)$	$b$
Value	0.1 $kg \cdot m^{2}$	0.04 $kg \cdot m^{2}$	0.07 $\ N - m\text{/}\text{rad}$	0	$0.1 N - s - m\text{/}\text{rad}$

# Exercise 1
using Plots;
pyplot();

using ControlSystems
using SymPy

(s, K, b, Ih, Im) = symbols("s K b I_h I_h")

K_v = 0.07; b_v = 0.1; Ih_v = 0.04; Im_v = 0.1;
A = [Im*s^2+b*s+K -b*s-K; -b*s-K Ih*s^2+b*s+K];
B = [1; 0];

Gx = inv(A)*B
G1s = (s^2*Gx[1])(K=>K_v, Im=>Im_v, Ih=>Ih_v, b=>b_v);
G2s = (s^2*Gx[2])(K=>K_v, Im=>Im_v, Ih=>Ih_v, b=>b_v);

# Takes a symbolic returns a ControlSystems::TransferFunction
function Sym_to_TF(G::Sym)::TransferFunction 
    s = symbols("s")
    num_s, den_s = simplify(G).as_numer_denom() # Grab num /den polynomials
    num = N(expand(num_s).as_poly(s).all_coeffs()) # Extract coefficient
    den = N(expand(den_s).as_poly(s).all_coeffs()) 
    num = round.(num; sigdigits = 3) # Round
    den = round.(den; sigdigits = 3)
    return tf(num, den) # Convert to ControlSystems::TransferFunction
end

G1  = Sym_to_TF(G1s)
@show G1s
G2  = Sym_to_TF(G2s)
@show G2s

t = 0:0.1:10;
p = stepplot(G2, t, lw=3)

G1s = s^2*(0.1*s^2 + 0.1*s + 0.07)/(0.01*s^4 + 0.02*s^3 + 0.014*s^2)
G2s = s^2*(0.1*s + 0.07)/(0.01*s^4 + 0.02*s^3 + 0.014*s^2)

EXERCISE 2

Use basic numerical integration to simulate the response of the system to a step input $M_{m}( t) = 2$ , note that this is a 2DOF system, so the numerical integration vector would be

\mathbf{x =}\begin{bmatrix} \begin{matrix} x_{1} \\ x_{2} \\ \end{matrix} \\ \begin{matrix} x_{3} \\ x_{4} \\ \end{matrix} \\ \end{bmatrix} = \begin{bmatrix} \begin{matrix} \theta_{m} \\ {\dot{\theta}}_{m} \\ \end{matrix} \\ \begin{matrix} \theta_{h} \\ {\dot{\theta}}_{h} \\ \end{matrix} \\ \end{bmatrix}

But you will need to keep track of acceleration as well. So, either create a 6xn state vector to store ${\ddot{\theta}}_{m}$ and ${\ddot{\theta}}_{h}$ or keep a separate array for the accelerations.

Plot the angular accelerations on one sublot, and the angular velocities on a second subplot.
Hint: You already have the EOM from task 1, construct the numerical integration xdot function from them.

You should get the same response from stepplot() if you specify $M_{m}(t) = 1$

# Exercise 2
function dxdt(x,u,t)
    K = 0.07; b = 0.1; I_h = 0.04; I_m = 0.1;
    dx = [0.0; 0.0; 0.0; 0.0];
    dx[1] = x[2];
    dx[2] = (u  - K * x[1] - b * x[2] + K * x[3] + b * x[4]) / I_m;
    dx[3] = x[4];
    dx[4] = (- K * x[3] - b * x[4] + K * x[1] + b * x[2]) / I_h; 
    return dx
end

# Integration Time step
dt = 0.1;
#  Time vector
t = 0:dt:20;
    
# Initialize x
x_sim = zeros(6,length(t)); # Empty 2xn array
ui = zeros(1,length(t)); # Empty 1xn vector

x0 = [0, 0, 0, 0, 0, 0]
x_sim[:,1] = x0;
ui = ones(1, length(t))
for ix = 1:length(t)
    xdot = dxdt(x_sim[1:4,ix], ui[1,ix], t[ix]); # Grab the derivative vector
    x_sim[5:6, ix] = [xdot[2]; xdot[4]];
    if ix < length(t)
        x_sim[1:4, ix+1] = x_sim[1:4, ix] + xdot * dt; # Integrate x
    end
end
 
# And plot
plot(t,ui[1,:])
p = plot(t,x_sim[5,:], lw=3, label = "\$\\ddot{\\theta}_m\$")
plot!(p, t,x_sim[6,:], lw=3, label = "\$\\ddot{\\theta}_h\$")

EXERCISE 3

From the response in the previous task, define a function stepinfo(G) to numerically compute the following performance values for a transfer function's step response: Rise Time $T_{r}$ , Peak Time $T_{p}$ , Settling Time $T_{s}$ , and Percent Overshoot $\% OS$ . Test the function on the step response of the head acceleration transfer function derived above, and display the computed results of these performance specifications on the figure.

Hints:

For Rise Time: You need to compute the time it takes to go from 10% to 90% of the final output
For %OS, find the max() value in the response and compare with the final value x(end)
For Peak Time: use the index of max() to find the corresponding time.
For Settling Time: Capture the last time the output is greater than 2% of final value.

# Exercise 3
rd(x) = round(x, sigdigits=3) 

function stepinfo(t::Array, x::Array)
    xmax = maximum(x)   
    # Max compared to final value
    OS = rd(xmax/x[end] - 1)
    # From index of max value
    Tp = rd(t[findfirst(a->a==xmax, x)[1]])
    # From index of first time to reach 10% and 90% of final value
    Tr = rd(t[findfirst(t->t>=0.9*x[end], x)[1]] - t[findfirst(t->t>=0.1*x[end], x)[1]])
    # From last index when value is ≥ 2% of final value 
    Ts = rd(t[findlast(t->abs(t-x[end])>=0.02, x)[1]+1])

    println("Rise Time: $Tr s")
    println("Percent Overshoot: $(OS*100)%")
    println("Peak Time: $Tp s")
    println("Settling Time: $Ts s")
    return Tr, OS, Tp, Ts
end

function stepinfo(G::TransferFunction)
    x, t, _  = step(G)
    return stepinfo(t[:,1],x[:,1])
end

# Test 
Ts = 2.5;
Tp = 1;
ωd = π / Tp;
σ = 4 / Ts;
ζ = cos(atan(ωd, σ))
ωn = σ/ζ
OS = exp(-ζ*π/sqrt(1-ζ^2))
s = tf("s")
G = ωn^2 / (s^2+2*ζ*ωn*s+ωn^2)

println("Analytical:")
println("Percent Overshoot: $(rd(OS*100))%")
println("Peak Time: $Tp s")
println("Settling Time: $Ts s")
println("\nMeasured:")

stepinfo(G)

Analytical:
Percent Overshoot: 20.2%
Peak Time: 1 s
Settling Time: 2.5 s

Measured:
Rise Time: 0.49 s
Percent Overshoot: 20.1%
Peak Time: 0.98 s
Settling Time: 2.38 s

Problem B. DC Motor Parameter Identification.

You are given a real brushed DC Motor, but you don't have its full specifications. You do; however, have a simple model for a DC motor and you are given three sets of experimental data for a motor:

i. The angular velocity (rad/s) of the motor in response to a step voltage input of 10V

ii. The torque-speed curve for the motor at a set input voltage of 5V.

iii. The torque-speed curve for the motor at a set input voltage of 10V.

Each of the above data sets are provided in a separate and labeled csv file, using the CSV.jl and DataFrames.jl packages. The below code snippet shows you how to read the data into Julia arrays.

using CSV, DataFrames
data = CSV.read("data.csv", DataFrame);
t = data[!,1];
y = data[!,2];

The Simple DC Motor transfer function is given as:

\frac{\Theta_{m}( s )}{E_{m}( s )} = \frac{K_{t}\text{/}(R_{a}J_{m})}{s\lbrack s + \frac{1}{J_{m}}(D_{m} + \frac{K_{t}K_{B}}{R_{a}})\rbrack}

which is equivalent to $\frac{\Theta_{m}( s )}{E_{m}( s )} = \frac{K}{s( s + \alpha )}$ , if all the constants are lumped. Note that the data set for the step response is given for angular velocity, so we need $\dfrac{\dot{\Theta}_m(s)}{E_m(s)}$ . This can be easily derived by differentiating $\Theta(s)$ the angular position, by multiplication with a differentiator $s$ , which gives the first order transfer function:

\dfrac{\dot{\Theta}_m( s)}{E_{m}(s)} = \frac{K}{( s + \alpha)}

And remember, for the torque-speed curve, we use the time domain relationship that relate input voltage, angular velocity and torque:

T_{m}( t ) = - \frac{K_{B}K_{t}}{R_{a}}\omega_{m}( t ) + \frac{K_{t}}{R_{a}}e_{a}( t )

EXERCISE 4

Obtain, from the step response data set, the values of $K$ and $\alpha$ in Equation 2.

The response is first order, if you find the time constant and scale the response you can find the two values $K$ and $\alpha$ . Hint: Apply the Final Value Theorem to find K

Obtain, from the two torque-speed curve data sets, the value of the coefficients in Equation 3. Use $R_{a} = 5$

Once plotted, you can perform this task using hand calculations, as covered in class.

From the previous two tasks, compute all the remaining coefficients, $J_{m}\,\&\, D_{m}$ , in Equation 1

Also covered in class.

Now that you can redefine your transfer function, simulate it to a step input using the step() command and compare it to the step response data in the provided data set. (Remember to scale by 10 to account for the value of voltage input of 10V and to use the transfer function $\dfrac{\dot{\Theta}_m(s)}{E_m(s)}$ , which is equal to $\dfrac{s\Theta_{m}( s )}{E_{m}( s )}$

# Exercise 4
using CSV, DataFrames
data = CSV.read("../data/Part_I_Problem_B_Data_Step_Response.csv", DataFrame);
t = data[!,1];
x = data[!,2];

r = 10
tau = t[findfirst(a->a>=0.6*x[end], x)[1]]; 
a = 1 / tau
K= a * x[end] / r;
s = tf("s")
G = K / (s+a)
p = plot(t, x, lw=3,  label="Experiment")
stepplot!(p, r*G, t, lw=3, label="Estimate")


data = CSV.read("../data/Part_I_Problem_B_Data_Torque_Speed_10V.csv", DataFrame);
w = data[!,1];
T = data[!,2];
e = 10;
Ra = 8;
wnoload = w[end]
Tstall = T[1]
Kt = rd(Ra * Tstall / e)
Kb = rd(e / wnoload)
Jm = rd(Kt / (Ra * K))
Dm = rd(a*Jm - Kt*Kb/Ra)
println("Motor Parameters:\nR_a=$Ra, K_t=$Kt, K_b=$Kb, J_m=$Jm, D_m=$Dm")

Motor Parameters:
R_a=8, K_t=1.92, K_b=1.0, J_m=1.8, D_m=0.578

PART II: INTRODUCTION TO FEEDBACK CONTROL – NUMERICAL IMPLEMENTATION

This part is meant to serve as a tutorial on how to numerically implement a PID controller in a simulation. A MATLAB template script is provided to get you started. Follow through and complete the tasks.

A PID controller takes the form shown on Figure 2. Using the transfer function form, we can come up with a reduced equivalent closed-loop form and simulate the response of the feedback system to an impulse, step or ramp response directly, and this is useful in a number of control design scenarios. The reduced form is shown on Figure 3

Figure 2 - PID Controller: Transfer Function Form

Figure 3 - PID Controller: Equivalent Reduced Closed-Loop Form

But as we discussed in the previous assignment, implementing the simulation using numerical integration is a much more flexible design platform. Let's first get a sense on how to implement the PID Controller using transfer functions and using the reduced closed-loop form.

EXERCISE 5

Given the system $G_{\text{ol}} = \dfrac{1}{( s^{2} + 10s + 20 )}$ , and the gain values: $K_{P} = 300,\ K_{D} = 1,K_{I} = 300$ . Simulate the closed-loop response of the system with the PID controller, to a step input using step().

Substitute the values into the closed-loop form and simulate.

# Exercise 5
s = tf("s")
Gp = 2 / (s^2+8*s+25);
Kp = 200; Ki = 150; Kd = 1;
Gc = (Kd*s^2 + Kp*s + Ki ) / s;

t = 0:0.01:10 
Gcl = feedback(Gc*Gp, 1);
p = stepplot(Gcl, t, lw=3);

To implement the PID Controller, or any controller, numerically, we must understand its logic in the time domain. Figure 4 shows the PID feedback controller expressed in the time domain.

The PID controller has three components: Proportional to Error, Proportional to Error Integral, Proportional to Error Derivative. So if we calculate the error, its integral and its derivative at every time sample we can compute the control law $u_{\text{PID}}(t)$ at every time sample (we assume a small enough simulation $\Delta t$ , otherwise we have to discuss digital controllers, which is outside the scope of this assignment).

u_{\text{PID}}( t ) = K_{P}e( t ) + K_{I}\int_{}^{}{e( t )} + K_{D}\frac{\text{de}( t )}{\text{dt}}

Figure 4 - PID Controller: Time Domain Form

EXERCISE 6

Using the provided script template, complete the algorithm definitions for

a. The first error value and first control law before the integration loop

b. The next error value in the integration loop: $e = r - x$

c. The next error integral in the integration loop: $\int_{}^{}e = ( \int_{}^{}e )_{\text{previous}} + e \cdot dt$

d. The next error derivative in the integration loop: $\frac{\text{de}}{\text{dt}} = ( e - e_{\text{previous}} )\text{/}\text{dt}$

e. The next control output $u$ in the integration loop

From Equation 4 above.

Then test your implementation of the numerical PID controller and compare the simulation output against the output you obtained using the closed-form method above. To troubleshoot your code, start first by applying the proportional gain to both methods (set $K_{I} = K_{D} = 0)$ , then add the integral gain (set $K_{D} = 0)$ , then add the derivative gain instead (set $K_{I} = 0)$ , then add all the gains.

# Exercise 6
# Funtion for xdot vector - using an anonymous function.
dxdt(x,u,t) = [x[2]; 2*u - 8 * x[2] - 25 * x[1]]

# Initialize simulation parameters
x0 = [0; 0]
dt = 0.01;
t_sim = 0:dt:2;
x_sim = zeros(2,length(t_sim)); # Empty 2xn array
u = zeros(length(t_sim)); # Empty 1xn vector
e = zeros(length(t_sim));
e_int = zeros(length(t_sim));
e_dot = zeros(length(t_sim)); 

# Reference input
r = 1; 
# Compute First Error
e[1] = 0;
# Compute First Control Output
u[1] = 0; 

x_sim[:,1] = x0;
Kp = 200; Ki = 150; Kd = 1;
 
for idx = 1:length(t_sim)-1
    # Grab the derivative vector
    xdot = dxdt(x_sim[:,idx], u[idx], t_sim[idx]);
    
    # Integrate 
    x_sim[:, idx+1] = x_sim[:, idx] + xdot * dt;

    # Calculate Errors
    e[idx+1] = r - x_sim[1, idx+1];
    e_int[idx+1] = e_int[idx] + e[idx]*dt;
    e_dot[idx+1] = (e[idx+1] - e[idx]) / dt;

    # PID Control Law
    u[idx+1] =  Kp*e[idx+1] + Ki*e_int[idx+1] + Kd*e_dot[idx+1];

end
 
# And plot
p = plot(t_sim,0.01*u, lw=3, label = "0.01u")
plot!(t_sim,x_sim[1,:], lw=3, label = "\$x_1\$")
plot!(p, t_sim,x_sim[2,:], lw=3, label = "\$x_2\$")

PART III: FEEDBACK CONTROL – VIA GAIN TUNING

A. DC Motor PID Speed and Torque Control – MATLAB Control Toolbox commands

In Part I.B you retrieved the model parameters for a DCmotor, you will attempt to implement a feedback controller on this system to

a. control the speed of the motor to a given step set-point,

b. control the torque of the motor to a given step set-point.

For this question, implement the feedback controller using the transfer function method explained in Part II. Then tune the gains to achieve the required performance specifications. Plot the responses and the performance specifications.

Note: With the parameters retrieved in Part I.B. In addition, use the inductance term here $L_{a}$ to achieve a more realistic transient response, with $\mathbf{L}_{\mathbf{a}}\mathbf{= 5}\mathbf{H}$ . The transfer functions are given.

EXERCISE 7

Using a PID Controller, implement it in a feedback loop to control the speed of a motor given the torque input. Attain the following performance specifications: $r(t) = 10rad\text{/}s$ , $T_{r} < 0.2s$ , $T_{p} < 0.5s$ , $\% OS < 12\%$ , $T_{s} < 0.6s$ . Plot the responses.

{\dfrac{\dot{\Theta}_m(s)}{E_{a}(s)} = \frac{K_{t}s}{(( Js^{2} + D_{m}s )( R_{a} + L_{a}s ) + K_{t}K_{b}s)\ }}

# Exercise 7

Kb = 1; Kt = 2; Ra = 8; J = 2; D = 0.6; La = 5;
s = tf("s");
G_speed = s * Kt / ( (J * s^2 + D * s) * (Ra + La *  s) + Kt * Kb * s);

# Gains
Kp = 10; Ki = 5; Kd = 10;
# Reference Input
r = 10;

t=0:.01:5;

Gc = Kp + Ki/s + Kd * s;
Gcl_speed = feedback(Gc*G_speed,1);
p = stepplot(r*Gcl_speed,t, lw=3, label="\$\\dot{θ}\$")
stepinfo(r*Gcl_speed)

Rise Time: 2.55 s
Percent Overshoot: 0.0%
Peak Time: 5.0 s
Settling Time: 4.85 s

In class, we discussed how we can represent the transfer function relating voltage to output torque

{\frac{T_{m}( s )}{E( s )} = \frac{K_{t}(Js^{2} + D_{m}s)}{( Js^{2} + D_{m}s )(R_{a} + L_{a}s) + K_{t}K_{b}s}}

EXERCISE 8

Use a PID controller in a feedback loop to control the Torque of the same motor. Attain the following performance specifications: $r( t ) = 4N.m,$ , $T_{r} < 0.1s$ , $T_{p} < 1s$ , $\% OS < 0.1\%$ , $T_{s} < 0.4s$ . Plot the responses.

This problem should not consume much time if you have completed Part II Task 1: Use the right transfer function for the system and tune the gains through trial and error and your understanding of what each term of the PID does to achieve the required performance. Use this exercise to gain an appreciation for how each PID controller term affects the response.

# Exercise 8

Kb = 1; Kt = 2; Ra = 8; J = 2; D = 0.6; La = 5;
s = tf("s");
G_torque = Kt * ( J * s^2 + D * s) / ( (J * s^2 + D * s) * (Ra + La *  s) + Kt * Kb * s)

# Gains
Kp = 110; Ki = 100; Kd = 0;
# Reference Input
r = 2;

t=0:.01:5;

Gc = Kp + Ki/s + Kd * s;
Gcl_torque = feedback(Gc*G_torque,1);

p = stepplot(r*Gcl_speed,t, lw=3, label="\$τ\$")
stepinfo(r*Gcl_speed)

Rise Time: 2.55 s
Percent Overshoot: 0.0%
Peak Time: 5.0 s
Settling Time: 4.45 s

B. Disk Drive Position Control – Numerical Integration**

In Part I Problem A, you were able to get a stable response for controlling the acceleration of the disk drive motor (The system $G_{1}( s ) = \dfrac{\ddot{\Theta}_m(s)}{M_{m}(s)}$ is stable), if we attempt to get the speed or position response with a step input and without feedback, the system then is unstable (The systems $G_{2}( s ) = \frac{\dot{\Theta}_m(s)}{M_m(s)}$ and $G_{3}( s ) = \frac{\Theta_m(s)}{M_m( s )}$ are unstable). We can use feedback control to stabilize the systems $G_{2}$ or $G_{3}$ .

EXERCISE 9

Use a PID Controller to achieve a stable position control response for the motor and attain the following performance specifications: $r( t ) = 1rad,$ , $T_{r} < 0.15s$ , $T_{p} < 2s$ , $\% OS < 13\%$ , $T_{s} < 2s$ .

Note: Use low values for the gains (start with values lower than 1)

Note that the input $M_{m}$ is applied on the same inertial mass we are trying to control the position of: $\theta_{m}(t)$ , we can try to control $\theta_{h}(t)$ directly by measuring its error instead and computing the control law output. Trying to control a part of a system (the disk drive head), while the actuation effort is at another part of the system (the motor) is termed non-collocated control, and can usually result in a nonminimum phase behavior. Trying to control an inverted pendulum's angle while applying force on the moving cart holding the pendulum, is another example of non-collocated control (it would be much easier to have a motor at the pendulum pivot: collocated control)

# Exercise 9
# Funtion for xdot vector - using an anonymous function.
function dxdt(x,u,t)
    K_v = 0.07; b_v = 0.1; Ih_v = 0.04; Im_v = 0.1;
    return [x[2]; 
    (u - K_v * x[1] - b_v * x[2] + K_v * x[3] + b_v * x[4]) / Im_v; 
    x[4];
    (-K_v * x[3] - b_v * x[4] + K_v * x[1] + b_v * x[2]) / Ih_v 
    ];
end  

# Initialize simulation parameters
x0 = [0; 0; 0; 0]
dt = 0.001;
t_sim = 0:dt:2;
x_sim = zeros(4,length(t_sim)); # Empty 2xn array
u = zeros(length(t_sim)); # Empty 1xn vector
e = zeros(length(t_sim));
e_int = zeros(length(t_sim));
e_dot = zeros(length(t_sim)); 

# Reference input
r = 1; 
# Compute First Error
e[1] = 0;
# Compute First Control Output
u[1] = 0; 

x_sim[:,1] = x0;
Kp = 10; Ki = 8; Kd = 1;
 
for idx = 1:length(t_sim)-1
    # Grab the derivative vector
    xdot = dxdt(x_sim[:,idx], u[idx], t_sim[idx]);
    # Integrate 
    x_sim[:, idx+1] = x_sim[:, idx] + xdot * dt;

    # Calculate Errors
    e[idx+1] = r - x_sim[1, idx+1];
    e_int[idx+1] = e_int[idx] + e[idx]*dt;
    e_dot[idx+1] = (e[idx+1] - e[idx]) / dt;

    # PID Control Law
    u[idx+1] =  Kp*e[idx+1] + Ki*e_int[idx+1] + Kd*e_dot[idx+1];

end
 
# And plot
p = plot(t_sim,0.05*u, lw=3, label = "0.05u")
plot!(t_sim,10*x_sim[1,:], lw=3, label = "\$10x_1\$")
plot!(p, t_sim,x_sim[2,:], lw=3, label = "\$x_2\$")

stepinfo(t_sim[:,1], x_sim[1,:])

Rise Time: 0.096 s
Percent Overshoot: 30.099999999999998%
Peak Time: 0.253 s
Settling Time: 0.76 s

Bonus Task: Instead of applying the PID on the motor angular position, try to apply it on the drive read head $\theta_{h}$ , try to come up with a stable response. Try to get the following performance specifications: $r(t) = 1rad,$ , $T_{r} < 4s$ , $T_{p} < 10s$ , $\% OS < 50\%$ , $T_{s} < 50s$ .

You will notice that with only PID control, we are better off implementing a collocated controller. There are more advanced ways of controlling non-collocated systems, which is the subject of another course.