Model obráceného kyvadla

Petr Bednář, Katedra elektroenergetiky, ČVUT FEL
bednarp@feld.cvut.cz

Zadané hodnoty: m - hmotnost kyvadla, M - hmotnost vozíku, L - délka kyvadla, g - gravitační zrychlení

zadani = {m -> 2, M -> 6, L -> 1, 
          g -> 9.81};

Obr. Obrácené kyvadlo

(Subscript[X, 0])- počáteční poloha vozíku, (Subscript[φ, 0])- počáteční výchylka kyvadla, (Subscript[X, k])- požadovaná poloha vozíku

rizeni = {α -> 3, 
          X0 -> -0.15, φ0 -> -0.05, 
          Xk -> 0.15};

Síly (Subscript[F, x]) a (Subscript[F, y]) působící na kyvadlo

rce1 = Fx == m×xc''[t];
rce2 = Fy - m×g == m×yc''[t];

Linearizace sin(φ) -> φ, cos(φ) -> 1

rce3 = (1/2) Fy×L×φ[t] - (1/2) 
              Fx×L == 
          J×φ''[t];

Moment setrvačnosti J

J = ((m×(L^2))/12);

Síly působící na vozík, (Subscript[f, x]) hnací síla

rce4 = fx - Fx == M×X''[t];

Poloha těžiště kyvadla

xc[t_] = 
        X[t] + (1/2) L×φ[t];
yc[t_] = (1/2) L;

Řešené diferenciální rovnice

rce = 
        Eliminate[{rce1, rce2, rce3, rce4}, {Fx, Fy}] // 
          Simplify;
rce[[1]]
rce[[2]]

Stavový model

x' = A . x + B . u
y = C . x

x - stavové veličiny (x, x', φ, φ')
u - akční veličina (fx)
y - výstup (x, φ)

x = {{X[t]}, {X'[t]}, {φ[t]}, {φ'[
              t]}};
u = {{fx}};
y = {{X[t]}, {φ[t]}};

Žádaná poloha vozíku s kyvadlem

xzad = {{Xk}, {0}, {0}, {0}} /. rizeni;

Určení matice A

A = Table[a[i, j], {i, Length[x]}, {j, Length[x]}];

xdos = Solve[
          Eliminate[rce, φ''[t]], X''[t]][[1, 
          1]]

φdos = Solve[
          Eliminate[rce, X''[t]], φ''[t]][[1, 
          1]]

Dxt = D[x, t] /. {xdos, φdos};

Table[
          Solve[Coefficient[((A . x - Dxt))[[i]], 
                x[[j]]] == 0, a[i, j]], {i, Length[x]}, {j, 
            Length[x]}] // Flatten;
A = A //. %;
A // MatrixForm

Určení matice B

B = Table[b[i, j], {i, Length[x]}, {j, Length[u]}];

Table[
          Solve[Coefficient[((B . u - Dxt))[[j]], 
                u[[i]]] == 0, b[j, i]], {i, Length[u]}, {j, 
            Length[x]}] // Flatten;
B = B //. %;
B // MatrixForm

Určení matice C

Cc = Table[c[i, j], {i, Length[y]}, {j, Length[x]}];

Table[
          Solve[Coefficient[(Cc . x - y)[[i]], 
                  x[[j]]][[1]] == 0], {i, Length[y]}, {j, 
            Length[x]}] // Flatten;
Cc = Cc //. %;
Cc // MatrixForm

Matice řiditelnosti R

R = 
        Transpose[
          Flatten[Table[
              Transpose[MatrixPower[A, i - 1] . B], {i, Length[A]}], 
            1]];
R // MatrixForm

MatrixRank[R]

Stavová zpětná vazba K (Ackermannova formule)

Ii = IdentityMatrix[4];
Ii // MatrixForm

Požadovaná dynamika M(A)

M[A_] = MatrixPower[A + α×Ii, 5];

Ackermannova formule

K = {{0, 0, 0, 1}} . Inverse[R] . M[A];

Řešení regulace s použitím stavové zpětné vazby

<< Graphics`Colors`
ClearAll[φ, fx]

Stavový vektor (Subscript[x, 0]) v čase (Subscript[t, 0])

x0 = {{X0}, {0}, {φ0}, {0}};

Řešení rovnice x' = A . x + B . u pro u = -K . x (pro požadovaný stav x = (([0 0 0 0])^T))
x(t) = (e^([A - B . K] . t)) . (Subscript[x, 0])

Pro konečný stav v hodnotě (Subscript[X, k]) ((Subscript[x, k]) = (([(Subscript[X, k]) 0 0 0])^T)) bude u = K . ((Subscript[x, k]) - x)
x(t) = 
        (e^([A - B . K] . t)) . ((Subscript[x, 0]) + (([A - B . K])^(-1)) . B . 
                  K . (Subscript[x, k])) - (([A - B . K])^(-1)) . B . K . (Subscript[x, k])

Pro výstup y
y(t) = C . x(t)

reseni = Cc . (MatrixExp[
                      Evaluate[(A - B . K) /. zadani]×t] . (x0 + 
                        Inverse[A - B . K] . B . K . xzad) - 
                  Inverse[A - B . K] . B . K . xzad) /. zadani /. 
          rizeni;
Xvysl[t_] = reseni[[1]] /. rizeni;
φvysl[t_] = 
        reseni[[2]] /. rizeni;
fxres[
          t_] = K . (xzad - x) /. zadani /. 
              rizeni /. {X -> 
                Xvysl, φ -> φvysl} // 
          Flatten;

SetOptions[Plot, AspectRatio ->  .25, 
        DisplayFunction -> Identity, PlotPoints -> 200, 
        AxesLabel -> {"t (s)", None}, 
        AxesOrigin -> {0, 0}];

tmax = 20;
grafx = 
        Plot[{Xvysl[t], Xk /. rizeni}, {t, 0, tmax}, PlotRange -> All, 
          PlotStyle -> {{ForestGreen, Thickness[ .004]}, {Black, 
                Dashing[{ .01,  .01}]}}, 
          PlotLabel -> "x (m)"];
\(\(graff = 
        Plot[fxres[t], {t, 0, tmax}, PlotRange -> All, 
          PlotStyle -> {Red, Thickness[ .004]}, 
          PlotLabel -> \*""\!\(f\_x\) (N)""];\)\)
grafφ = 
        Plot[Evaluate[φvysl[t]], {t, 0, tmax}, 
          PlotRange -> All, 
          PlotStyle -> {RoyalBlue, Thickness[ .004]}, 
          PlotLabel -> "φ (-)"];

Show[GraphicsArray[{{grafx}, {graff}, {grafφ}}], 
        DisplayFunction -> $DisplayFunction, 
        ImageSize -> 500];

Grafické znázornění pohybu vozíku

<<Graphics`Animation`

\(\(ani = 
        Table[Graphics[{Line[{{\(-1\), 0}, {1, 0}}], 
              Line[{{\(-1\), \(- .05\)}, {\(-1\),  .05}}], 
              Line[{{0, \(- .05\)}, {0,  .05}}], 
              Line[{{1, \(- .05\)}, {1,  .05}}], 
              Line[{{\(Xvysl[t]\)[\([1]\)],  .1}, {\(Xvysl[t]\)[\([1]\)] + 
                      Sin[\(φvysl[t]\)[\([1]\)]],  .1 + 
                      Cos[\(φvysl[t]\)[\([1]\)]]}}], 
              Rectangle[{\(Xvysl[t]\)[\([1]\)] -  .06,  .02}, {\(Xvysl[
                        t]\)[\([1]\)] +  .06,  .1}], 
              Circle[{\(Xvysl[t]\)[\([1]\)] -  .035,  .02},  .02], 
              Circle[{\(Xvysl[t]\)[\([1]\)] +  .035,  .02},  .02], 
              Rectangle[{\(-1\), \(- .4\)}, {\(-1\) + 2  t/tmax, \(- .3\)}], 
              Line[{{\(-1\), \(- .4\)}, {1, \(- .4\)}, {1, \(- .3\)}, {\(-1\), \(- .3\)}}], 
              Text["0:" <> 
                  ToString[IntegerPart[t]], {\(- .8\), \(- .2\)}, {\(-1\), 
                  0}], Text[
                ToString[
                    Round[100*\(Re[
                            Xvysl[t]]\)[\([1]\)]]] <> " cm", {0, \(- .2\)}, {0, 0}], 
              Text[ToString[ .1*
                      Round[10*\(180\/π\) \(Re[φvysl[
                                t]]\)[\([1]\)]]] <> "°", {0.8, \(- .2\)}, {1, 0}]}, PlotRange -> {{\(-1.1\), 1.1}, {\(- .6\), 1.2}}, 
            AspectRatio -> Automatic, ImageSize -> 300], {t, 0, 
            tmax,  .1}];\)\)
ShowAnimation[Table[ani[[i]], {i, Length[ani]}]]