Κώδικας:
In[1]:= A = ( {
{0, -1, 0, 0, 0, 0},
{Kt/mu, -(Ct + Cs)/mu, -Ks/mu, Cs/mu, -Ap/mu, 0},
{0, 1, 0, -1, 0, 0},
{0, Cs/ms, Ks/ms, -Cs/ms, Ap/ms, 0},
{0, Ap*a, 0, -Ap*a, -a*Ctm, a*Cd*Sqrt[Ps/p]*w},
{0, 0, 0, 0, 0, -1/T}
} )
B = ( {
{0},
{0},
{0},
{0},
{0},
{k/T}
} )
c = ( {
{0, 0, 0, 0, 0, 1}
} )
Out[1]= {{0, -1, 0, 0, 0, 0}, {Kt/mu, (-Cs - Ct)/mu, -(Ks/mu), Cs/
mu, -(Ap/mu), 0}, {0, 1, 0, -1, 0, 0}, {0, Cs/ms, Ks/ms, -(Cs/ms),
Ap/ms, 0}, {0, a Ap, 0, -a Ap, -a Ctm, a Cd Sqrt[Ps/p] w}, {0, 0, 0,
0, 0, -(1/T)}}
Out[2]= {{0}, {0}, {0}, {0}, {0}, {k/T}}
Out[3]= {{0, 0, 0, 0, 0, 1}}
In[4]:= Ap = 0.0044
Cd = 0.7
Ctm = 15*10^-12
Cs = 12000
Ct = 200
k = 1481
Ks = 240
Kt = 1000
kv = 2.1
ms = 2800
mu = 270
Ps = 20684
w = 0.008
p = 3500
a = 2.273*10^9
T = 1/15
v = 1
Out[4]= 0.0044
Out[5]= 0.7
Out[6]= 3/200000000000
Out[7]= 12000
Out[8]= 200
Out[9]= 1481
Out[10]= 240
Out[11]= 1000
Out[12]= 2.1
Out[13]= 2800
Out[14]= 270
Out[15]= 20684
Out[16]= 0.008
Out[17]= 3500
Out[18]= 2.273*10^9
Out[19]= 1/15
Out[20]= 1
In[21]:= PID = Kp + Ki/s + Kd*s
Out[21]= Kp + Ki/s + Kd s
In[22]:= Pcl =
Collect[Simplify[s*Det[s*IdentityMatrix[6] - A + B.c*PID]], s]
Out[22]= 240.451 Ki +
1. (0.162357 + 1.31221*10^6 Ki + 240.451 Kp) s +
1. (886.044 + 240.451 Kd + 618596. Ki + 1.31221*10^6 Kp) s^2 +
1. (476.757 + 1.31221*10^6 Kd + 4.18171*10^6 Ki + 618596. Kp) s^3 +
1. (2851.42 + 618596. Kd + 1.09975*10^6 Ki + 4.18171*10^6 Kp) s^4 +
1. (930.813 + 4.18171*10^6 Kd + 22215 Ki + 1.09975*10^6 Kp) s^5 +
1. (64.505 + 1.09975*10^6 Kd + 22215 Kp) s^6 + 1. (1 + 22215 Kd) s^7
In[23]:=
S7 = (Pcl)[[8, 2]]
S6 = (Pcl)[[7, 2]]
S5 = (Pcl)[[6, 2]]
S4 = (Pcl)[[5, 2]]
S3 = (Pcl)[[4, 2]]
S2 = (Pcl)[[3, 2]]
S1 = (Pcl)[[2, 2]]
S0 = (Pcl)[[1]]
Out[23]= 1 + 22215 Kd
Out[24]= 64.505 + 1.09975*10^6 Kd + 22215 Kp
Out[25]= 930.813 + 4.18171*10^6 Kd + 22215 Ki + 1.09975*10^6 Kp
Out[26]= 2851.42 + 618596. Kd + 1.09975*10^6 Ki + 4.18171*10^6 Kp
Out[27]= 476.757 + 1.31221*10^6 Kd + 4.18171*10^6 Ki + 618596. Kp
Out[28]= 886.044 + 240.451 Kd + 618596. Ki + 1.31221*10^6 Kp
Out[29]= 0.162357 + 1.31221*10^6 Ki + 240.451 Kp
Out[30]= 240.451 Ki
Routh = ( {
{S7, S5, S3, S1},
{S6, S4, S2, S0},
{b1, b2, b3, 0},
{c1, c2, c3, 0},
{d1, d2, 0, 0},
{e1, e2, 0, 0},
{f1, 0, 0, 0},
{S0, 0, 0, 0}
} )
b1 = -(Det[Routh[[{1, 2}, {1, 2}]]]/S6)
b2 = -(Det[Routh[[{1, 2}, {1, 3}]]]/S6)
b3 = -(Det[Routh[[{1, 2}, {1, 4}]]]/S6)
c1 = -(Det[Routh[[{2, 3}, {1, 2}]]]/b1)
c2 = -(Det[Routh[[{2, 3}, {1, 3}]]]/b1)
c3 = -(Det[Routh[[{2, 3}, {1, 4}]]]/b1)
d1 = -(Det[Routh[[{3, 4}, {1, 2}]]]/c1)
d2 = -(Det[Routh[[{3, 4}, {1, 3}]]]/c1)
e1 = -(Det[Routh[[{4, 5}, {1, 2}]]]/d1)
e2 = -(Det[Routh[[{4, 5}, {1, 3}]]]/d1)
f1 = -(Det[Routh[[{5, 6}, {1, 2}]]]/e1)
S0 = S0 = (Out[23])[[1]]
ex = Expand[Pd = (s*10)^2*(s + 3)^3*3*(s + 1)^2 + s*10 + 150]
CoeffPcl = CoefficientList[Pcl, s]
CoeffPd = CoefficientList[Pd, s]
FindInstance[{CoeffPcl[[1]] - CoeffPd[[1]] == 0}, {Ki, Kp, Kd}]
FindInstance[{CoeffPcl[[2]] - CoeffPd[[2]] == 0}, {Kp, Kd, Ki}]
FindInstance[{CoeffPcl[[3]] - CoeffPd[[3]] == 0}, {Kd, Ki, Kp}]
FindInstance[{CoeffPcl[[4]] - CoeffPd[[4]] == 0}, {Ki, Kp, Kd}]
FindInstance[{CoeffPcl[[5]] - CoeffPd[[5]] == 0}, {Ki, Kp, Kd}]
FindInstance[{CoeffPcl[[6]] - CoeffPd[[6]] == 0}, {Ki, Kp, Kd}]
FindInstance[{CoeffPcl[[7]] - CoeffPd[[7]] == 0}, {Ki, Kp, Kd}]
FindInstance[{CoeffPcl[[8]] - CoeffPd[[8]] == 0}, {Ki, Kp, Kd}]
KiKpKd = FindInstance[
Reduce[S7 > 0 && S6 > 0 && S0 > 0 && b1 > 0 && c1 > 0 && d1 > 0 &&
e1 > 0 && f1 > 0, {Ki, Kp, Kd}], {Ki, Kp, Kd}]