Äîìàøíåå çàäàíèå ïî êóðñó "Èíôîðìàöèîííûå

òåõíîëîãèè ìàòåìàòè÷åñêîãî ìîäåëèðîâàíèÿ"

Êîæàíîâ Å.Ì. ÂÔÍ12-71

3 ñåíòÿáðÿ 2013 ã.

Ñîäåðæàíèå

1 Çàäàíèå 2

2 Óðàâíåíèÿ äâèæåíèÿ â ôîðìå Êîøè 3

3 Ñèñòåìà ïåðâîãî ïðèáëèæåíèÿ, îïèñûâàþùàÿ äâèæåíèå ìåõàíèç-ìà 3

4 Ñòàáèëèçàöèÿ ïîëîæåíèÿ ðàâíîâåñèÿ ñ íàáëþäàòåëåì ïîëíîãî ïî-ðÿäêà 3

5 Ïðîâåðêà ïîëó÷åííîãî ðåøåíèÿ (âû÷èñëèòåëüíûé ýêñïåðèìåíò) 5

6 Ïðèëîæåíèå 1. Êîä ïðîãðàììû íà ÿçûêå Python 8

7 Ïðèëîæåíèå 2. Êîä ìîäóëÿ ïðîãðàììû íà ÿçûêå Matlab 10

1

1 Çàäàíèå

Äëÿ ìàÿòíèêà ñ ìàõîâèêîì (ñì. ðèñ.),

äâèæåíèå êîòîðîãî çàïèñûâàåòñÿ ñ ïîìîùüþ óðàâíåíèéD(q)q +G(q) = Q,

ãäå q =(q1q2

), D(q) =

(m1l

2c1 +m2l

21 + I1 + I2 I2

I2 I2

)

G(q) =(

−mgsinq10

), Q =

(τ10

), 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

2 Óðàâíåíèÿ äâèæåíèÿ â ôîðìå Êîøè

Ïðèâåä¼ì èìåþùèåñÿ óðàâíåíèÿ äâèæåíèÿ ê ôîðìå Êîøè. Ïðèìåì Θ =m1l

2c1 +m2l

21 + I1 + I2.

detD = (Θ − I2)I2

D−1 = I2detD

(1 −1

−1 Θ/I2

)= 1

Θ−I2

(1 −1

−1 Θ/I2

)q = D−1Q−D−1G(q)

D−1Q = 1Θ−I2

(1 −1

−1 Θ/I2

)(τ10

)= τ1

Θ−I2

(1

−1

)

D−1G = 1Θ−I2

(1 −1

−1 Θ/I2

)(−mgsinq1

0

)= mgsinq1

Θ−I2

(−11

)Èòàê, óðàâíåíèÿ â ôîðìå Êîøè:

p1 = τ1−mgsinq1Θ−I2

p2 = −τ1+mgsinq1Θ−I2

p1 = q1p2 = q2

3 Ñèñòåìà ïåðâîãî ïðèáëèæåíèÿ, îïèñûâàþùàÿ äâèæå-íèå ìåõàíèçìà

Ïðèíèìàÿ, ÷òî sinq1 ≈ q1, ïîëó÷àåì ñõåìó ïåðâîãî ïðèáëèæåíèÿ, îïè-ñûâàþùóþ äâèæåíèå ìåõàíèçìà:

p1 = τ1−mgq1Θ−I2

p2 = −τ1+mgq1Θ−I2

p1 = q1p2 = q2

4 Ñòàáèëèçàöèÿ ïîëîæåíèÿ ðàâíîâåñèÿ ñ íàáëþäàòåëåìïîëíîãî ïîðÿäêà

Íåîáõîäèìî: ñòàáèëèçèðîâàòü ïîëîæåíèå ðàâíîâåñèÿ q = 0, èñïîëüçóÿíàáëþäàòåëü ïîëíîãî ïîðÿäêà ïî âûõîäó y1 = q2, y2 = q2 è ïîëàãàÿm1 = 12êã, m2 = 8êã, l1 = 1ì, lc1 = 0.5ì, I1 = 1êã·ì2, I2 = 1êã·ì2.

Ïóñòü ξ =

p1p2q1q2

.Òîãäà óðàâíåíèå ñèñòåìû áóäåò âûãëÿäåòü ñëåäóþùèì îáðàçîì:

3

p1p2q1q2

= A

p1p2q1q2

+Bτ1

ãäå A =

0 0 −mg

Θ−I20

0 0 mgΘ−I2

01 0 0 00 1 0 0

, B =

1

Θ−I21Θ−I2

00

.Íàáëþäàòåëü ïîëíîãî ïîðÿäêà ïî âûõîäó ìîæåò áûòü çàäàí â âèäå âåê-

òîðà: y =(q1q2

)=

0p20q2

.

Åñëè çàïèñàòü ÷åðåç âåêòîð ξ: y = Cξ, ãäå C =

0 0 0 00 1 0 00 0 0 00 0 0 1

.Òàêæå íàáëþäàòåëü ïîëíîãî ïîðÿäêà ìîæåò áûòü çàäàí äâóìÿ âåêòîðà-ìè:

y1 = q2 =(

0 0 0 1)

p1p2q1q2

= C1ξ, y2 = q2 =(

0 1 0 0)

p1p2q1q2

=

C2ξ ðåçóëüòàòå ïîëó÷àåì ñèñòåìó:ξ = Aξ +Buy1 = C1ξy2 = C2ξ

u = KξrÂåêòîð îøèáêè: e = ξ−ξr, ãäå ξr = Aξr+Bu+L1(C1ξr−y1)+L2(C2ξr−y2)Ïðîâîäèì ïðåîáðàçîâàíèÿ:ξ = (A+BK)ξ −BKee = ξ − ξ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+XTBT +BX ≺ 0Q � 0

Äëÿ âòîðîé ñèñòåìû ïðîâîäÿòñÿ ïðåîáðàçîâàíèÿ:(A+ L1C1 + L2C2)TR+R(A+ L1C1 + L2C2) ≺ 0ATR+RA+ CT1 L

T1 R+ CT2 L

T2 R+RL1C1 +RL2C2 ≺ 0

Çàìåíà ïåðåìåííûõ: RL1 = Y1, RL2 = Y2.

4

{ATR+RA+ CT1 Y

T1 + CT2 Y

T2 + Y1C1 + Y2C2 ≺ 0

R � 0

5 Ïðîâåðêà ïîëó÷åííîãî ðåøåíèÿ (âû÷èñëèòåëüíûé ýêñ-ïåðèìåíò)

Ðåøàÿ çàäà÷ó LMI, ïîëó÷àåì ñëåäóþùèé ðåçóëüòàò:

K =(

254.73734601 11.78931085 858.59504097 4.04721376)

L1 =

73.6261

−51.11198.7012−0.5

L2 =

16.5875−3.970511.523925.5125

T =

(A+BK −BK

0 A+ L1C1 + L2C2

)=

=

−21.228 −0.982 −60.105 −0.337 21.228 0.982 71.550 0.337275.965 12.772 918.700 4.384 −275.965 −12.772 −930.145 −4.3841.000 0.000 0.000 0.000 −0.000 −0.000 −0.000 −0.0000.000 1.000 0.000 0.000 −0.000 −0.000 −0.000 −0.0000.000 0.000 0.000 0.000 0.000 16.587 11.445 73.6260.000 0.000 0.000 0.000 0.000 −3.970 −11.445 −51.1120.000 0.000 0.000 0.000 1.000 11.524 0.000 8.7010.000 0.000 0.000 0.000 0.000 26.512 0.000 −0.500

Ìîäåëèðîâàíèå ïðîâîäèì â ñëåäóþùåé ïîñëåäîâàòåëüíîñòè:

• Çàäà¼ì íà÷àëüíîå çíà÷åíèå âåêòîðà

(ξe

).

• Âû÷èñëÿåì "ñêîðîñòü èçìåíåíèÿ âåêòîð

(ξe

).

• Âû÷èñëÿåì çíà÷åíèå âåêòîðà

(ξe

)íà ñëåäóþùèé ìîìåíò âðåìå-

íè:

(ξe

)=(ξe

)∆t (çäåñü ∆t - øàã ìîäåëèðîâàíèÿ).

• Ïîâòîðÿåì ïðåäûäóùèå äâà øàãà äî îêîí÷àíèÿ ïåðèîäà ýêñïåðè-ìåíòà.

5

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

6

7

6 Ïðèëîæåíèå 1. Êîä ïðîãðàììû íà ÿçûêå Python

Ê ñîæàëåíèþ, âòîðîå íåðàâåíñòâî íå óäàëîñü ðåøèòü ñ ïîìîùüþ áèá-ëèîòåêè picos, ïîýòîìó äàííûé ó÷àñòîê êîäà çàêîììåíòèðîâàí:

__author__ = 'eugeny'

import picos as picimport scipy as simport numpy as npfrom numpy import linalg as la

m1, m2 = 12.0, 8.0l1, lc1 = 1.0, 0.5I1, I2 = 1.0, 1.0g = 9.81

tetta = m1 * lc1 * lc1 + m2 * l1 * l1 + I1 + I2m_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], ]).Tn = 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

# 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 * C2p << 0)# 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 MatlabP = 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 = ", Kprint "L1 = ", L1print "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) + np.dot(L2, C2)]])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

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.0t_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 pltplt.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.save�g('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

lmiterm([-lmi1 1 1 q],1,1); % 0<Q

lmi2 = newlmi;lmiterm([lmi2 1 1 q],1,A,'s'); % QA + A'Qlmiterm([lmi2 1 1 y1],C1',1,'s'); % C1'Y1' + Y1C1lmiterm([lmi2 1 1 y2],C2',1,'s'); % C2'Y2' + Y2C2

lmisys = getlmis;

[tmin,xfeas] = feasp(lmisys);if tmin<0

Q = dec2mat(lmisys,xfeas,q);Y1 = dec2mat(lmisys,xfeas,y1);Y2 = dec2mat(lmisys,xfeas,y2);P = inv(Q);K1 = P*Y1'K2 = P*Y2'

elseK1 = []; K2 = []; P = [];

endelse

error('Ìàòðèöà A íå ÿâëÿåòñÿ êâàäðàòíîé!')end

11