Para el problema de Segway

El problema de control de Segway se puede modelar de la siguiente manera en torno a un punto de equilibrio $x_e = (p,0,0,0)$. Por simplicidad se tomo que no se tiene fricción lineal ni rotacional ($c = \gamma = 0$). Las matrices que modelan la dinḿica del sistema se pueden expresar: $$\mathbf{A}=\left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & & 1\\ 0 & m^2l^2g/\mu & -cJt/\mu & -\gamma l m/\mu\\ 0 & M_t m g l/\mu & -c l m/\mu & -\gamma M_t/\mu\\ \end{array}\right],\qquad \mathbf{B}=\left[\begin{array}{c} 0 \\ 0 \\ J_t/\mu\\ lm/\mu \end{array}\right]$$

$$\mathbf{C}=\left[1\quad 0\quad 0\quad 0\right],\qquad D=0$$

Donde: $M = 10$, Masa de la base en $kg$.

$m = 80$, masa de la persona en $kg$.

$l = 1$ centro de masa de la persona en $kg$.

$J = 100$ inercia de la persona en $kg m^2/s^2$.

$c = 0.1$ amortiguamiento traslacional $Ns/m$.

$\gamma= 0.01$ amortiguamiento rotacional $N m s$.

$g = 9.8$ constante gravitacional $m/s^2$.

Otras constantes del sistema son:

$Mt = M + m$ masa total.

$Jt = J + ml^2$ inercia total.

$\mu = M_t J_t - m^2 l^2$ común denominador.

El sistema se define el el archivo de matlab dado por la cátedra.


In [1]:
M = 10;					% mass of base in kg
m = 80;					% mass of rider in kg
l = 1;  				% center of mass of rider in m
J = 100;				% inertia of rider, kg m^2/s^2
c = 0.1;				% translational damping, Ns/m
gam = 0.01;				% rotational damping, N m s
g = 9.8;				% gravitational constant m/s^2

% Derived quantities
Mt = M + m;				% total mass
Jt = J + m*l^2;				% total inertia
det = Mt*Jt - m^2 * l^2;		% common denominator

In [7]:
% System dynamics
A = [
  0  0              1          0;
  0  0              0          1;
  0  m^2*l^2*g/det  -c*Jt/det  -gam*l*m/det;
  0  Mt*m*g*l/det   -c*l*m/det -gam*Mt/det
];
B = [0; 0; Jt/det; l*m/det];
C = [1 0 0 0];

% Compute the open loop eigenvalues and print them as a check for the text
sys=ss(A,B,C,0)
polos=eigs(A)
zer=tzero(sys)


sys =
 
  a = 
               x1          x2          x3          x4
   x1           0           0           1           0
   x2           0           0           0           1
   x3           0         6.4   -0.001837  -8.163e-05
   x4           0         7.2  -0.0008163  -9.184e-05
 
  b = 
             u1
   x1         0
   x2         0
   x3   0.01837
   x4  0.008163
 
  c = 
       x1  x2  x3  x4
   y1   1   0   0   0
 
  d = 
       u1
   y1   0
 
Continuous-time state-space model.


polos =

   -2.6837
    2.6829
   -0.0011
         0


zer =

    2.0870
   -2.0870

In [2]:
zeta0 = 0.5
omega0 = sqrt(m*g*l/(J+m*l^2))
zeta1 = 0.7 
omega1 = 0.5
lam3 = -zeta1*omega1 + omega1*sqrt(1-zeta1^2)*i
lam4 = -zeta1*omega1 - omega1*sqrt(1-zeta1^2)*i
evdes = [-1 + 2i, -1 - 2i, lam3, lam4]


zeta0 =

    0.5000


omega0 =

    2.0870


zeta1 =

    0.7000


omega1 =

    0.5000


lam3 =

  -0.3500 + 0.3571i


lam4 =

  -0.3500 - 0.3571i


evdes =

  -1.0000 + 2.0000i  -1.0000 - 2.0000i  -0.3500 + 0.3571i  -0.3500 - 0.3571i

In [8]:
K1 = place(A, B, evdes)
kr = -1/(C * inv(A-B*K1) * B);


K1 =

   1.0e+03 *

   -0.0156    1.7318   -0.0501    0.4432

In [17]:
sys = ss(A-B*K1, B*kr, [C; -K1], [0; kr]);
[y,t]=step(sys);
subplot(2,1,1),plot(t,y(:,1)),grid;
subplot(2,1,2),plot(t,y(:,2)),grid;



In [18]:
eig(A-B*K1)


ans =

  -1.0000 + 2.0000i
  -1.0000 - 2.0000i
  -0.3500 + 0.3571i
  -0.3500 - 0.3571i

In [19]:
zeta0 = zeta0; omega0 = omega0/3;
zeta1 = zeta1; omega1 = omega1/2;
Tmax = 40;
lam3 = -zeta1*omega1 + omega1*sqrt(1-zeta1^2)*i;
lam4 = -zeta1*omega1 - omega1*sqrt(1-zeta1^2)*i;
evdes = [-zeta0*omega0 + omega0*i, -zeta0*omega0 - omega0*i, lam3, lam4]

% Design the controller and construct the closed loop dynamics
K2 = place(A, B, evdes)
kr = -1/(C * inv(A-B*K2) * B);


evdes =

  -0.3478 + 0.6957i  -0.3478 - 0.6957i  -0.1750 + 0.1785i  -0.1750 - 0.1785i


K2 =

   -0.4726  994.6516   -3.2901  135.2605

In [20]:
sys = ss(A-B*K2, B*kr, [C; -K2], [0; kr]);

In [23]:
[Y, T] = step(sys, Tmax);
subplot(2,1,1),plot(T,Y(:,1)),grid;
subplot(2,1,2),plot(T,Y(:,2)),grid;



In [ ]: