Домашнее задание, в котором приводится простой пример стабилизации системы на основе решения линейных матричных неравенств (LMI).

1. Äîìàøíåå çàäàíèå ïî êóðñó "Èíôîðìàöèîííûå
òåõíîëîãèè ìàòåìàòè÷åñêîãî ìîäåëèðîâàíèÿ"
Êîæàíîâ Å.Ì. ÂÔÍ12-71
3 ñåíòÿáðÿ 2013 ã.
Ñîäåðæàíèå
1 Çàäàíèå 2
2 Óðàâíåíèÿ äâèæåíèÿ â ôîðìå Êîøè 3
3 Ñèñòåìà ïåðâîãî ïðèáëèæåíèÿ, îïèñûâàþùàÿ äâèæåíèå ìåõàíèç-
ìà 3
4 Ñòàáèëèçàöèÿ ïîëîæåíèÿ ðàâíîâåñèÿ ñ íàáëþäàòåëåì ïîëíîãî ïî-
ðÿäêà 3
5 Ïðîâåðêà ïîëó÷åííîãî ðåøåíèÿ (âû÷èñëèòåëüíûé ýêñïåðèìåíò) 5
6 Ïðèëîæåíèå 1. Êîä ïðîãðàììû íà ÿçûêå Python 8
7 Ïðèëîæåíèå 2. Êîä ìîäóëÿ ïðîãðàììû íà ÿçûêå Matlab 10
1

2. 1 Çàäàíèå
Äëÿ ìàÿòíèêà ñ ìàõîâèêîì (ñì. ðèñ.),
äâèæåíèå êîòîðîãî çàïèñûâàåòñÿ ñ ïîìîùüþ óðàâíåíèé
D(q)¨q + G(q) = Q,
ãäå q =
q1
q2
, D(q) =
m1l2
c1
+ m2l2
1 + I1 + I2 I2
I2 I2
G(q) =
− ˜mgsinq1
0
, Q =
τ1
0
, ˜m = m1lc1 + m2l1,
m1 - ìàññà ìàÿòíèêà, m2 - ìàññà ìàõîâèêà, I1 - ìîìåíò èíåðöèè ìàÿòíèêà
îòíîñèòåëüíî åãî öåíòðà ìàññ, I2 - ìîìåíò èíåðöèè ìàõîâèêà, l1 - äëèíà
ìàÿòíèêà, lc1 - ðàññòîÿíèå îò øàðíèðà äî öåíòðà ìàññ ìàÿòíèêà, q1 - óãîë
ìåæäó ìàÿòíèêîì è ëó÷îì, íàïðàâëåííûì âåðòèêàëüíî ââåðõ, q2 - óãîë
ìåæäó ìàÿòíèêîì è ìàõîâèêîì, τ1 - óïðàâëÿþùèé ìîìåíò, ïðèëîæåííûé
ê ìàõîâèêó.
Òðåáóåòñÿ:
• çàïèñàòü äàííûå óðàâíåíèÿ äâèæåíèÿ â ôîðìå Êîøè;
• ñîñòàâèòü ñèñòåìó ïåðâîãî ïðèáëèæåíèÿ, îïèñûâàþùóþ äâèæåíèå
ìåõàíèçìà;
• ñòàáèëèçèðîâàòü ïîëîæåíèå ðàâíîâåñèÿ q = 0, èñïîëüçóÿ íàáëþ-
äàòåëü ïîëíîãî ïîðÿäêà ïî âûõîäó y1 = q2, y2 = ˙q2 è ïîëàãàÿ
m1 = 12êã, m2 = 8êã, l1 = 1ì, lc1 = 0.5ì, I1 = 1êã·ì2, I2 = 1êã·ì2;
• ïðîâåðèòü ïîëó÷åííîå ðåøåíèå ñ ïîìîùüþ âû÷èñëèòåëüíîãî ýêñ-
ïåðèìåíòà, ïðåäúÿâèòü ãðàôèêè ïåðåõîäíûõ ïðîöåññîâ çàòóõàíèÿ
âîçìóùåíèé.
2

3. 2 Óðàâíåíèÿ äâèæåíèÿ â ôîðìå Êîøè
Ïðèâåä¼ì èìåþùèåñÿ óðàâíåíèÿ äâèæåíèÿ ê ôîðìå Êîøè. Ïðèìåì Θ =
m1l2
c1
+ m2l2
1 + I1 + I2.
detD = (Θ − I2)I2
D−1 = I2
detD
1 −1
−1 Θ/I2
= 1
Θ−I2
1 −1
−1 Θ/I2
¨q = D−1Q − D−1G(q)
D−1Q = 1
Θ−I2
1 −1
−1 Θ/I2
τ1
0
= τ1
Θ−I2
1
−1
D−1G = 1
Θ−I2
1 −1
−1 Θ/I2
− ˜mgsinq1
0
= ˜mgsinq1
Θ−I2
−1
1
Èòàê, óðàâíåíèÿ â ôîðìå Êîøè:


˙p1 = τ1− ˜mgsinq1
Θ−I2
˙p2 = −τ1+ ˜mgsinq1
Θ−I2
p1 = ˙q1
p2 = ˙q2
3 Ñèñòåìà ïåðâîãî ïðèáëèæåíèÿ, îïèñûâàþùàÿ äâèæå-
íèå ìåõàíèçìà
Ïðèíèìàÿ, ÷òî sinq1 ≈ q1, ïîëó÷àåì ñõåìó ïåðâîãî ïðèáëèæåíèÿ, îïè-
ñûâàþùóþ äâèæåíèå ìåõàíèçìà:


˙p1 = τ1− ˜mgq1
Θ−I2
˙p2 = −τ1+ ˜mgq1
Θ−I2
p1 = ˙q1
p2 = ˙q2
4 Ñòàáèëèçàöèÿ ïîëîæåíèÿ ðàâíîâåñèÿ ñ íàáëþäàòåëåì
ïîëíîãî ïîðÿäêà
Íåîáõîäèìî: ñòàáèëèçèðîâàòü ïîëîæåíèå ðàâíîâåñèÿ q = 0, èñïîëüçóÿ
íàáëþäàòåëü ïîëíîãî ïîðÿäêà ïî âûõîäó y1 = q2, y2 = ˙q2 è ïîëàãàÿ
m1 = 12êã, m2 = 8êã, l1 = 1ì, lc1 = 0.5ì, I1 = 1êã·ì2, I2 = 1êã·ì2.
Ïóñòü ξ =





p1
p2
q1
q2





.
Òîãäà óðàâíåíèå ñèñòåìû áóäåò âûãëÿäåòü ñëåäóþùèì îáðàçîì:
3

4. 




˙p1
˙p2
˙q1
˙q2





= A





p1
p2
q1
q2





+ Bτ1
ãäå A =





0 0 − ˜mg
Θ−I2
0
0 0 ˜mg
Θ−I2
0
1 0 0 0
0 1 0 0





, B =





1
Θ−I2
1
Θ−I2
0
0





.
Íàáëþäàòåëü ïîëíîãî ïîðÿäêà ïî âûõîäó ìîæåò áûòü çàäàí â âèäå âåê-
òîðà: y =
q1
˙q2
=





0
p2
0
q2





.
Åñëè çàïèñàòü ÷åðåç âåêòîð ξ: y = Cξ, ãäå C =





0 0 0 0
0 1 0 0
0 0 0 0
0 0 0 1





.
Òàêæå íàáëþäàòåëü ïîëíîãî ïîðÿäêà ìîæåò áûòü çàäàí äâóìÿ âåêòîðà-
ìè:
y1 = q2 = 0 0 0 1





p1
p2
q1
q2





= C1ξ, y2 = ˙q2 = 0 1 0 0





p1
p2
q1
q2





=
C2ξ
 ðåçóëüòàòå ïîëó÷àåì ñèñòåìó:


˙ξ = Aξ + Bu
y1 = C1ξ
y2 = C2ξ
u = Kξr
Âåêòîð îøèáêè: e = ξ−ξr, ãäå ξr = Aξr +Bu+L1(C1ξr −y1)+L2(C2ξr −y2)
Ïðîâîäèì ïðåîáðàçîâàíèÿ:
˙ξ = (A + BK)ξ − BKe
˙e = ˙ξ − ˙ξr = Aξ + Bu = Aξr − Bu − L1(C1ξr − y1) − L2(C2ξr − y2) =
A(ξ − ξr) + L1C1(ξ − ξr) + L2C2(ξ − ξ2) = (A + L1C1 + L2C2)e
˙ξ
˙e
=
A + BK −BK
0 A + L1C1 + L2C2
ξ
e
Ïåðâàÿ ñèñòåìà ïðèíèìàåò âèä:
QAT + AQ + XT BT + BX 0
Q 0
Äëÿ âòîðîé ñèñòåìû ïðîâîäÿòñÿ ïðåîáðàçîâàíèÿ:
(A + L1C1 + L2C2)T R + R(A + L1C1 + L2C2) 0
AT R + RA + CT
1 LT
1 R + CT
2 LT
2 R + RL1C1 + RL2C2 0
Çàìåíà ïåðåìåííûõ: RL1 = Y1, RL2 = Y2.
4

5. AT R + RA + CT
1 Y T
1 + CT
2 Y T
2 + Y1C1 + Y2C2 0
R 0
5 Ïðîâåðêà ïîëó÷åííîãî ðåøåíèÿ (âû÷èñëèòåëüíûé ýêñ-
ïåðèìåíò)
Ðåøàÿ çàäà÷ó LMI, ïîëó÷àåì ñëåäóþùèé ðåçóëüòàò:
K = 254.73734601 11.78931085 858.59504097 4.04721376
L1 =





73.6261
−51.1119
8.7012
−0.5





L2 =





16.5875
−3.9705
11.5239
25.5125





T =
A + BK −BK
0 A + L1C1 + L2C2
=
=














−21.228 −0.982 −60.105 −0.337 21.228 0.982 71.550 0.337
275.965 12.772 918.700 4.384 −275.965 −12.772 −930.145 −4.384
1.000 0.000 0.000 0.000 −0.000 −0.000 −0.000 −0.000
0.000 1.000 0.000 0.000 −0.000 −0.000 −0.000 −0.000
0.000 0.000 0.000 0.000 0.000 16.587 11.445 73.626
0.000 0.000 0.000 0.000 0.000 −3.970 −11.445 −51.112
0.000 0.000 0.000 0.000 1.000 11.524 0.000 8.701
0.000 0.000 0.000 0.000 0.000 26.512 0.000 −0.500














Ìîäåëèðîâàíèå ïðîâîäèì â ñëåäóþùåé ïîñëåäîâàòåëüíîñòè:
• Çàäà¼ì íà÷àëüíîå çíà÷åíèå âåêòîðà
ξ
e
.
• Âû÷èñëÿåì "ñêîðîñòü èçìåíåíèÿ âåêòîð
˙ξ
˙e
.
• Âû÷èñëÿåì çíà÷åíèå âåêòîðà
ξ
e
íà ñëåäóþùèé ìîìåíò âðåìå-
íè:
ξ
e
=
˙ξ
˙e
∆t (çäåñü ∆t - øàã ìîäåëèðîâàíèÿ).
• Ïîâòîðÿåì ïðåäûäóùèå äâà øàãà äî îêîí÷àíèÿ ïåðèîäà ýêñïåðè-
ìåíòà.
5

6. Ðåçóëüòàòû ìîäåëèðîâàíèÿ äëÿ îòêëîíåíèé â 0.01 ðàä, 0.2 ðàä è 0.5
ðàä ñîîòâåòñòâåííî:
6

7. 7

8. 6 Ïðèëîæåíèå 1. Êîä ïðîãðàììû íà ÿçûêå Python
Ê ñîæàëåíèþ, âòîðîå íåðàâåíñòâî íå óäàëîñü ðåøèòü ñ ïîìîùüþ áèá-
ëèîòåêè picos, ïîýòîìó äàííûé ó÷àñòîê êîäà çàêîììåíòèðîâàí:
__author__ = 'eugeny'
import picos as pic
import scipy as s
import numpy as np
from numpy import linalg as la
m1, m2 = 12.0, 8.0
l1, lc1 = 1.0, 0.5
I1, I2 = 1.0, 1.0
g = 9.81
tetta = m1 * lc1 * lc1 + m2 * l1 * l1 + I1 + I2
m_tilda = m1 * lc1 + m2 * l1
C1 = np.mat([[0.0, 0.0, 0.0, 1.0], ])
C2 = np.mat([[0.0, 1.0, 0.0, 0.0], ])
A = np.mat([[0.0, 0.0, m_tilda * g / (tetta - I2), 0.0],
[0.0, 0.0, - m_tilda * g / (tetta - I2), 0.0],
[1.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0]])
B = np.mat([[-1.0 / (tetta - I2), tetta / (I2 * (tetta - I2)), 0.0, 0.0], ]).T
n = len(A)
m = len(A[0])
Ap = pic.new_param('A', A)
Bp = pic.new_param('B', B)
Ep = pic.new_param('E', s.eye(n))
sdp = pic.Problem()
Xp = sdp.add_variable('X', (1, n))
Qp = sdp.add_variable('Q', (n, n), vtype='symmetric')
sdp.add_constraint(Qp >> Ep)
sdp.add_constraint(Qp * Ap.T + Ap * Qp + Xp.T * Bp.T + Bp * Xp << 0)
sdp.solve()
Q = np.mat(Qp.value)
X = np.mat(Xp.value)
P = la.inv(Q)
K = X * P
8

9. # Ap = pic.new_param('A', A)
# C1p = pic.new_param('C1', C1)
# C2p = pic.new_param('C2', C2)
# Ep = pic.new_param('E', s.eye(n))
# sdp = pic.Problem()
# Y1p = sdp.add_variable('Y1', (n, 1))
# Y2p = sdp.add_variable('Y2', (n, 1))
# Rp = sdp.add_variable('R', (n, n), vtype='symmetric')
# sdp.add_constraint(Rp >> Ep)
# sdp.add_constraint(Ap.T * Rp + Rp * Ap + C1p.T * Y1p.T + Y1p * C1p + C2p.T * Y2p.T + Y2p *
# print sdp
# sdp.solve()
# Y1 = np.mat(Y1p.value)
# Y2 = np.mat(Y2p.value)
# R = np.mat(Rp.value)
# print Y1
# print Y2
# L1 = la.pinv(R) * Y1
# L2 = la.pinv(R) * Y2
# results from Matlab
P = np.mat([[7.6100, -2.3982, 1.5662, 0.0000],
[-2.3982, 1.6648, -0.2834, -0.0000],
[1.5662, -0.2834, 1.2924, 0.0000],
[0.0000, -0.0000, 0.0000, 0.8636]])
L1 = np.mat([[73.6261], [-51.1119], [8.7012], [-0.5000]])
L2 = np.mat([[16.5875], [-3.9705], [11.5239], [25.5125]])
print "K = ", K
print "L1 = ", L1
print "L2 = ", L2
Tetta = np.bmat([[A + np.dot(B, K), - np.dot(B, K)], [np.zeros((len(A), len(A))), A + np.dot(L1, C1) +
print "Tetta: "
for i in Tetta.tolist():
for j in i:
print "%.3f & " % j,
print " "
eps = [[0.0], [0.0], [0.5], [0.0]]
e = [[0.0], [0.0], [0.0], [0.0]]
epse = np.bmat([[eps], [e]])
t_angle = [[0, ], [epse[2].tolist()[0][0], ]]
9

10. t_omega = [[0, ], [epse[0].tolist()[0][0] / 10.0, ]]
t_angle2 = [[0, ], [epse[3].tolist()[0][0], ]]
t_omega2 = [[0, ], [epse[1].tolist()[0][0], ]]
for t in range(1, 300):
epse_di = np.dot(Tetta, epse)
epse += epse_di / 100.0
t_angle[1].append(epse[2].tolist()[0][0])
t_angle[0].append(t / 100.0)
t_omega[1].append(epse[0].tolist()[0][0] / 10.0)
t_omega[0].append(t / 100.0)
t_angle2[1].append(epse[3].tolist()[0][0])
t_angle2[0].append(t / 100.0)
t_omega2[1].append(epse[1].tolist()[0][0])
t_omega2[0].append(t / 100.0)
import matplotlib.pyplot as plt
plt.plot(t_angle[0], t_angle[1], 'b')
plt.plot(t_omega[0], t_omega[1], 'r')
# plt.plot(t_angle2[0], t_angle2[1], 'g')
# plt.plot(t_omega2[0], t_omega2[1], 'y')
plt.saveg('stab_angle.png')
7 Ïðèëîæåíèå 2. Êîä ìîäóëÿ ïðîãðàììû íà ÿçûêå Matlab
Ïðèâåä¼í êîä ðåøåíèÿ òîëüêî âòîðîãî íåðàâåíñòâà, îñòàëüíîå ïðîäåëàíî
â Python:
A = [0 0 11.445 0;0 0 -11.445 0;1 0 0 0;0 1 0 0];
B = [-0.0833333;1.08333333;0.01;0.01];
C1 = [0 0 0 1];
C2 = [0 1 0 0];
setlmis([]);
szA = size(A);
szC = size(C1);
n = szA(1);
if min(szA) == max(szA) length(szA) == 2
q = lmivar(1,[n 1]);
y1 = lmivar(2,szC);
y2 = lmivar(2,szC);
lmi1 = newlmi;
10

11. lmiterm([-lmi1 1 1 q],1,1); % 0Q
lmi2 = newlmi;
lmiterm([lmi2 1 1 q],1,A,'s'); % QA + A'Q
lmiterm([lmi2 1 1 y1],C1',1,'s'); % C1'Y1' + Y1C1
lmiterm([lmi2 1 1 y2],C2',1,'s'); % C2'Y2' + Y2C2
lmisys = getlmis;
[tmin,xfeas] = feasp(lmisys);
if tmin0
Q = dec2mat(lmisys,xfeas,q);
Y1 = dec2mat(lmisys,xfeas,y1);
Y2 = dec2mat(lmisys,xfeas,y2);
P = inv(Q);
K1 = P*Y1'
K2 = P*Y2'
else
K1 = []; K2 = []; P = [];
end
else
error('Ìàòðèöà A íå ÿâëÿåòñÿ êâàäðàòíîé!')
end
11