In the following post I will present a speed controller of the brushed DC motor using LQR. I will also introduce the Kalman filter to estimate the motor speed and torque.
 |
| System diagram |
Discrete state tracking linear quadratic regulator
Have the following linear discrete system
$$x_{k+1}=A_kx_k+B_ku_k\\
y_k=H_kx_k\tag{1}$$
Let us presume that $z_k$ is the trajectory what we would like to follow, in this case we have to determine the optimal control signal which optimize the quadratic criterion:
$$
J(u) = min\left[e_N^TFe_N+\sum_k{e_k^TQ_ke_k+\delta u_k^TR_k\delta u_k}\right]\tag{2}
$$
Where $e=z_k-y_k$ and $\delta u_k=u_k-up_k$.
Bellman's equation of the corresponding control problem is:
$$
J_{min_k}(u) = min\left[e_k^TQ_ke_k+\delta u_k^TR_k\delta u_k+J_{min_{k+1}}(u)\right]\tag{3}
$$
Expressing the cost of termination point as:
$$
J_{min_N}(u)=\left[z_N-H_Nx_N\right]^TF\left[z_N-H_Nx_N\right]\tag{4}
$$Introduce the following expressions:
$$P_N=H_N^TF_NH_N\\ p_{N_0}=z_N^TFz_N\\ g_N^T=-2z_N^TFH_N\tag{5}$$
Where $N$ is the number of the horizon and $k$ is the iteration.
The cost function will be the following:
$$
J_{min_k}(u)=p_{k_0}+g_k^Tx_k+x_k^TP_kx_k\tag{6}
$$
where the Belman's equation is:
$$
J_{min_k}(u)=min\left[e_k^TQ_ke_k+\delta u_k^TR_k\delta u_k+p_{(k+1)_0}+g_{k+1}^Tx_{k+1}+x_{k+1}^TP_kx_{k+1}\right]\tag{7}
$$
Considering that in the $k$ iteration the $u_k$ optimal control minimizes the criterion $J(u)_k$, we can write the following relation
$$u_k=\left[R_k+B_k^TP_{k+1}B_k\right]^{-1}\left[R_kup_k+\frac{1}{2}B_kg_{k+1}-B_k^TP_{k+1}A_kx_k\right]\tag{8} $$
The recursive discrete Ricatti equation is the following:$$P_k=H_k^TQ_kH_k-A_k^TP_{k+1}B_k\left[R_k+B_k^TP_{k+1}B_k\right]^{-1}\\B_k^TP_{k+1}A_k+A_k^TP_{k+1}A_k\tag{9}$$respecting the final condition:
$$P_N=H_N^TF_NH_N\tag{10}$$
We can express the optimal control signal in the following form:
$$u_k=K_kx_k+f_k\tag{11}$$
where
$$K_k=-\left[R_k+B_k^TP_{k+1}B_k\right]^{-1}B_k^TP_{k+1}A_k\tag{12}$$
$$f_k=-\frac{1}{2}\left[R_k+B_k^TP_{k+1}B_k\right]^{-1}\left[B_k^Tg_{k+1}\right]\tag{13}$$
After performing some algebraic calculations, we will end up with the following equation:$$g_k^T=-2z_k^TQ_kH_k+g_{k+1}\left[A_k+B_kK_k\right]^T\tag{14}$$
with boundary condition:
$$g_N^T=-2z_N^TFH_k\tag{15}$$
The controller signal is depending of the accuracy of the state estimator
Choosing the number of the horizon more than 15, the steady state error was 0. The number of the horizon, and the limitation of the control signal are influences the overshoot. One of the disadvantages of the LQR is that the optimal signal calculation doesn't include the signal limitation, thus a bigger control signal can be calculated than you can physically achieve.
The other is the LQR state tracker controller with Kalman filter
y_k=H_kx_k\tag{1}$$
Let us presume that $z_k$ is the trajectory what we would like to follow, in this case we have to determine the optimal control signal which optimize the quadratic criterion:
$$
J(u) = min\left[e_N^TFe_N+\sum_k{e_k^TQ_ke_k+\delta u_k^TR_k\delta u_k}\right]\tag{2}
$$
Where $e=z_k-y_k$ and $\delta u_k=u_k-up_k$.
Bellman's equation of the corresponding control problem is:
$$
J_{min_k}(u) = min\left[e_k^TQ_ke_k+\delta u_k^TR_k\delta u_k+J_{min_{k+1}}(u)\right]\tag{3}
$$
Expressing the cost of termination point as:
$$
J_{min_N}(u)=\left[z_N-H_Nx_N\right]^TF\left[z_N-H_Nx_N\right]\tag{4}
$$Introduce the following expressions:
$$P_N=H_N^TF_NH_N\\ p_{N_0}=z_N^TFz_N\\ g_N^T=-2z_N^TFH_N\tag{5}$$
Where $N$ is the number of the horizon and $k$ is the iteration.
The cost function will be the following:
$$
J_{min_k}(u)=p_{k_0}+g_k^Tx_k+x_k^TP_kx_k\tag{6}
$$
where the Belman's equation is:
$$
J_{min_k}(u)=min\left[e_k^TQ_ke_k+\delta u_k^TR_k\delta u_k+p_{(k+1)_0}+g_{k+1}^Tx_{k+1}+x_{k+1}^TP_kx_{k+1}\right]\tag{7}
$$
Considering that in the $k$ iteration the $u_k$ optimal control minimizes the criterion $J(u)_k$, we can write the following relation
$$u_k=\left[R_k+B_k^TP_{k+1}B_k\right]^{-1}\left[R_kup_k+\frac{1}{2}B_kg_{k+1}-B_k^TP_{k+1}A_kx_k\right]\tag{8} $$
The recursive discrete Ricatti equation is the following:$$P_k=H_k^TQ_kH_k-A_k^TP_{k+1}B_k\left[R_k+B_k^TP_{k+1}B_k\right]^{-1}\\B_k^TP_{k+1}A_k+A_k^TP_{k+1}A_k\tag{9}$$respecting the final condition:
$$P_N=H_N^TF_NH_N\tag{10}$$
We can express the optimal control signal in the following form:
$$u_k=K_kx_k+f_k\tag{11}$$
where
$$K_k=-\left[R_k+B_k^TP_{k+1}B_k\right]^{-1}B_k^TP_{k+1}A_k\tag{12}$$
$$f_k=-\frac{1}{2}\left[R_k+B_k^TP_{k+1}B_k\right]^{-1}\left[B_k^Tg_{k+1}\right]\tag{13}$$
After performing some algebraic calculations, we will end up with the following equation:$$g_k^T=-2z_k^TQ_kH_k+g_{k+1}\left[A_k+B_kK_k\right]^T\tag{14}$$
with boundary condition:
$$g_N^T=-2z_N^TFH_k\tag{15}$$
Results
The steady state error depends on the number of the horizon ($N$), the accuracy of the system model and the state estimation. Choosing the number of the horizon to five the rotor speed is the following:
Choosing the number of the horizon more than 15, the steady state error was 0. The number of the horizon, and the limitation of the control signal are influences the overshoot. One of the disadvantages of the LQR is that the optimal signal calculation doesn't include the signal limitation, thus a bigger control signal can be calculated than you can physically achieve.
Python implementation
The Python implementation of the state tracking LQR is under the following link. Feel free to fork. It was harder to implement into Python than Matlab, but finally I succeeded. In the T_DCMotor.py you can find two controller, one is a PID controllerpid = PID( KP = 0.2, KI = 100, KD = 0.00001
for i in range( len( pw ) ):
#Calculate the error
error = pw[i] - sw;
#Calculating the control signal
u = pid.run( error, Ts = Ts )
if( abs( u ) > 220 ):
u = sign( u ) * 220;
#Simulating
( sw, x ) = dc2.dlsim( u , torque[0, i ] );
y_out[i, :] = sw
u_out[i, :] = u
The other is the LQR state tracker controller with Kalman filter
dlqr = DSTLQR( Ak, Bk, Ck, N )
kf = dKF( x_est, Ak, Bk, H )
for i in range( len( pw ) - N ):
#Calculation the optimal control signal
U = dlqr.calcU( x_est, array( [pw[i:i + N]] ) )
if( abs( U ) > 220 ):
U = sign( U ) * 220;
#Simulating
( sw, x ) = dc2.dlsim( U, torque[0, i ], Ts = Ts );
y_out[i, :] = sw
u_out[i, :] = U
#Estimating the states
( x_est, P, error ) = kf.update( x[0, 0] , U )
y_est[i, :] = x_est[1];
Matlab source code
I used a discrete Kalman filter to estimate the torque and the rotor speed, here is the code of the Kalman filter:% Created by Róbert Ungurán
% GPLv2
%******************************************************
% Dc motor discrete model
%******************************************************
% Input
% I_in - Measured current
% U_in - Measured voltage
%******************************************************
% Output
% x_out - Estimated state variables
function x_out = kf(I_in,U_in)
global Ts Rm L Ke Kd J Km;
persistent x
persistent P
if isempty(x)
x = [0;0; 0];
P = zeros(3);
end
% Armature current, rotor speed, Torque
Ak = [1-Ts*(Rm/L) -Ts*(Ke/L) 0;
Ts*(Km/J) 1-Ts*(Kd/J) -Ts/J;
0 0 1;];
Bk = [Ts*1/L; 0; 0];
H = [1 0 0];
R = 1e-3;
Q = diag([1e-6 1e-2 1e-2]);
x = Ak*x + Bk*U_in;
P = Ak * P * Ak' + Q;
% Compute Kalman gain factor:
S = H*P*H'+R;
K = P*H'*inv(S);
% Correction based on observation:
error = I_in-H*x;
x = x + K*error;
P = P - K*H*P;
x_out = x;
end
The source of the discrete linear quadratic regulator
% Created by Róbert Ungurán
% GPLv2
%******************************************************
% LQR controller of a DC motor
%******************************************************
% Input
% I - Measured current
% w - Measured rotor speed
% w_ref - Reference rotor speed
%******************************************************
% Output
% U - Control signal
function U = mydlqr(I,w,Tl,w_ref)
global Ts Rm L Ke Kd J Km N;
x = [I;w;Tl];
Ak = [1-Ts*(Rm/L) -Ts*(Ke/L) 0;
Ts*(Km/J) 1-Ts*(Kd/J) -Ts/J;
0 0 1;];
Bk = [Ts*1/L; 0; 0];
Ck = [0 1 0];
Dk = [0];
[n,m] = size(Bk);
[p,n] = size(Ck);
R=eye(m)*0.001;
Q=eye(p)*10;
F=Q;
PN = Ck'*F*Ck;
gN = (-2*w_ref(1)'*F*Ck)';
for j=N:-1:1
K = (-inv(R+Bk'*PN*Bk)*Bk'*PN*Ak);
fk = (-1/2)*inv(R+Bk'*PN*Bk)*Bk'*gN;
gN = (-2*w_ref(:,N-1)'*Q*Ck+gN'*(Ak+Bk*K))';
PN = Ck'*Q*Ck-Ak'*PN*Bk*inv(R+Bk'*PN*Bk)*Bk'*PN*Ak+Ak'*PN*Ak;
end
U = K*x+fk;
end
The main source code
clear all
close all
clc
%% Initialization
global Ts Rm L Ke Kd J Km N Fc;
Rm = 0.35;
Km = 0.0296;
Ke = 0.0296;
Kd = 6.7*10e-5;
J = 2.9*10e-6;
Fc = 0.02;
L = 25 *10e-6;
Ts = 1e-5;
N = 5;
x = [0;0;0];
w_ref = [ones(1,1000)*200 ones(1,1000)*400];
torque = [ones(1,1500)*2.0 ones(1,500)*1.0];
[n,m] = size(w_ref);
u = [];
w = [];
x_est = [0; 0; 0];
x_est_o = [0; 0; 0];
for i=1:m-N
%% Calculation the optimal control signal
%Input Current, Rotation speed, Rotation speed references
U = mydlqr(x_est(1),x_est(2),x_est(3), w_ref(i:i+N));
if(abs(U) > 380)
U = sign(U)*380;
end
u = [u U];
%% Simulation the model
%Current, Rotation speed, Rotation position
[x(1), x(2), x(3)] = dc_motor(U, torque(i));
w = [w x(2)];
%% Estimating the states
x_est = kf(x(1), U);
x_est_o = [x_est_o x_est];
end
%% Visualization
plot(u);
figure;
plot(x_est_o(3,:));
figure;
hold on;
plot(w_ref, 'b-');
plot(w, 'r-');
plot(x_est_o(2,:), 'g-');
legend('Reference','Simulated', 'Estimated');
No comments:
Post a Comment