Motor model
The state space model of a brushed DC motor as follow:$$A =\left[ {\begin{array}{cc} -\frac{R_{a}}{L_{a}} & -\frac{K_{e}}{L_{a}} & 0 \\ \frac{K_{m}}{J} & -\frac{b}{J} & 0 \\ 0 & 1 & 0 \end{array} } \right] B=\left[ {\begin{array}{cc} \frac{1}{L_{a}} \\ 0 \\ 0 \end{array} } \right] C=\left[ {\begin{array}{cc} 0 \\ 1 \\ 0 \end{array} } \right] D=\left[ {\begin{array}{cc} 0 \end{array} } \right]\tag{1}$$
As you can see, I assumed that the torque is zero.
Where the states are:
$$\dot{x(t)} =\left[ {\begin{array}{cc} \frac{di}{dt} \\ \frac{d\omega}{dt} \\ \frac{d\theta}{dt} \end{array} } \right]\tag{2}$$
Controller design
I already introduced the MPC controller, please read that first before you continue to read this blog.
The first step is to determine the Toeplitz matrix:
$$\begin{bmatrix}x_{k+1|k} \\ \vdots \\ x_{k+N_c|k} \\ x_{k+N_c+1|k} \\
\vdots \\ x_{k+N|k} \end{bmatrix}=\begin{bmatrix}\Phi \\ \vdots \\
\Phi^{N_c} \\ \Phi^{N_c+1} \\
\vdots \\ \Phi^N \end{bmatrix}x_k + \begin{bmatrix}\Gamma \\ \vdots \\
\sum_{i=0}^{N_c-1} \Phi^{i} \Gamma\\ \sum_{i=0}^{N_c}\Phi^{i} \Gamma\\
\vdots \\ \sum_{i=0}^{N}\Phi^i \Gamma \end{bmatrix}u_{k-1}\\+
\begin{bmatrix}\Gamma & \cdots & 0 \\ \vdots & \ddots &
\vdots \\ \sum_{i=0}^{N_c}\Phi^{i} \Gamma & \cdots & \Phi
\Gamma+\Gamma \\
\vdots & \ddots & \vdots \\ \sum_{i=0}^{N-1}\Phi^i \Gamma &
\cdots & \sum_{i=0}^{N-N_c}\Phi^i \Gamma \end{bmatrix}
\begin{bmatrix}\Delta u_{k+1|k} \\ \vdots \\ \Delta u_{k+N_c-1|k}
\end{bmatrix}\tag{1}$$
I wrote the following Matlab function to build up the $\Phi^*$, $\Gamma^*$, $G_y$:
Motor parameters:function [FiFi,FiGam,Gy] = getFiFi_FiGa(Fi,Ga,C,N,Nc,n)
FiFi=[]; %ennek a merete N*n x n kell legyen
for i=1:N,
FiFi=[FiFi; C*Fi^i];
end
% FiGam szamitasa
FiGam=[];
for i=1:N,
S=0;
for j=0:i-1,
S=S+C*Fi^j*Ga;
end
FiGam=[FiGam; S];
end
ng=size(FiGam,1);
mg=size(FiGam,2);
% Gy szamitasa
Gy=zeros(ng,mg*Nc);
% Gy szamitasa
for j=1:mg:Nc*mg % oszlopok
Gy(j:end, j:j+mg-1)=FiGam(1:end-j+1,:);
end
end
To find the optimal control signal we have to solve the following optimization problem $J_k(u,x_k)=\frac{1}{2}\Delta u_k^TH\Delta u_k+f^T\Delta u_k+const$. I already presented the solution of this problem (see Model predictive control). I am going to implement the calculation of the $\Delta u_k$ optimal control signal in the following example:
In the example above I also tried to constrain the control signal by choosing the $lim$ variable to 38. After choosing the values of the noise matrices as follows
We can reduce the error by increasing or eliminating the constrain of control signal as you can see in $Figure 4$
The other solution of the optimal control signal calculation in Matlab is the built in $quadprog$ function. You can find a simple example below, also you can read the description of this function on the Matlab help page:
for v=1:n-N
%Error vector
E=Yref(v:v+N-1)-FiFi*x0-FiGam*u;
deltau=inv(Gy'*Q*Gy+R)*Gy'*Q*E;
u=u+deltau(1:2);
for k=1:length(u)
if(abs(u(k))>lim)
u(k) = sign(u(k))*lim;
end
end
x0 = Fi*x0+Ga*u;
yt = C*x0;
um=[um u];
ym=[ym;yt];
end;
In the example above I also tried to constrain the control signal by choosing the $lim$ variable to 38. After choosing the values of the noise matrices as follows
- $Q=eye(N*Nro)*1e4$ where $N$ is the number of the horizon and $Nro$ is the number of the outputs.
- $R=eye(Nc*Nrin)*1e-5$ where $Nc$ is the number of the control horizon and $Nrin$ is the number of the inputs.
 |
| Figure 1. Rotor speed |
 |
| Figure 2. Control signal |
 |
| Figure 3. Error between the reference and measured rotor speed |
 |
| Figure 4. Error between the reference and measured rotor speed without limiting the control signal |
H=2*(Gy'*Q*Gy+R);
for v=1:n-N
%Hiba vektor
E=Yref(v:v+N-1)-FiFi*x0-FiGam*u;
f=-2*E'*Q*Gy;
b = [abs(lim-u(1));
abs(lim+u(1))];
A = [1 0 0 0 0 0 0 0;
-1 0 0 0 0 0 0 0;
];
Aeq=[];beq=[];
[deltau,FVAL,EXITFLAG]=quadprog(H,f,A,b,Aeq,beq);
u=u+deltau(1:2);
x0 = Fi*x0+Ga*u;
yt = C*x0;
um=[um u0];
ym=[ym;yt];
Em = [Em;E];
end;
Appendix
Rm = 0.35;
Km=0.0296;
Ke = 0.0296;
b = 6.7*10e-5;
J = 2.9*10e-6;
Fc = 0.02;
L = 25 *10e-6;
Ts = 1e-4;
