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)
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]
In [8]:
K1 = place(A, B, evdes)
kr = -1/(C * inv(A-B*K1) * B);
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)
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);
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 [ ]: