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.


In [1]:
import control as ctrl
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline

Genero funciones que usare despues. Primero es la de las figuras


In [2]:
def create_figure(y1,t1,title=""):
    "Función para generar las figuras de este trabajo.\n"

    fig1=plt.figure()
    axes1 = fig1.add_axes([0.1, 0.1, 0.8, 0.8])
    axes1.plot(t1,y1[:,0],'b-')
    axes1.set_ylabel('Position (m)',color='b')
    axes1.set_xlabel('Time(s)')
    axes1.grid(which='both')
    axes1.set_title(title)
    for tl in axes1.get_yticklabels():
        tl.set_color('b')
    
    
    axes1i=axes1.twinx()
    axes1i.plot(t1,y1[:,1],'r-')
    axes1i.set_ylabel('Input Force (N)',color='r')
    axes1i.grid(which='both')
   
    for tl in axes1i.get_yticklabels():
        tl.set_color('r')
    return [fig1]

Genero funciones que me seran de ayuda para obtener $G(-s)$.


In [3]:
def pol_minus_s(pol):
        "Hago los polinomios"
        l=len(pol)
        npol=np.zeros((l))
        for i in xrange(1,l+1):
            if ((l-i)%2==0):
                npol[l-i]=pol[l-i]
            elif ((l-i)%2==1):
                npol[l-i]=-pol[l-i]
        return [npol]
        
def G_minus_s(G):
    "Función para generar G(-s), y poder hacer el SRL"
    num=pol_minus_s(G.num[0][0])
    den=pol_minus_s(G.den[0][0])
    Gms=ctrl.tf(num[0],den[0])
    return [Gms]

Parameter definitions (roughly a human on a Segway)


In [4]:
# Parameter definitions (roughly a human on a Segway)
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*np.power(l,2);				# total inertia
det = Mt*Jt - np.power(m,2) * np.power(l,2);		# common denominator

# balance_place.m - stabilization of balance system by pole placement

In [5]:
# System dynamics
A = np.matrix([[0., 0., 1., 0.],
              [0., 0., 0., 1.],
              [0., np.power(m,2)*np.power(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 = np.matrix([[0.],[0.],[Jt/det],[l*m/det]]);
C = np.matrix([[1., 0., 0, 0.]]);

# Compute the open loop eigenvalues and print them as a check for the text
poles=np.linalg.eigvals(A)
print poles

sys=ctrl.ss(A,B,C,[0.])


[  0.00000000e+00   2.68287302e+00  -2.68369048e+00  -1.11111099e-03]

Primer diseno con objetivos de control agresivos.


In [6]:
zeta0 = 0.5
omega0 = np.sqrt(m*g*l/(J+m*np.power(l,2)))
zeta1 = 0.7
omega1 = 0.5
lam3 = -zeta1*omega1 + omega1*np.sqrt(1-np.power(zeta1,2))*1j;
lam4 = -zeta1*omega1 - omega1*np.sqrt(1-np.power(zeta1,2))*1j;
evdes1 = [-1. + 2j, -1. - 2j, lam3, lam4]

Design the controller and construct the closed loop dynamics


In [7]:
K1 = ctrl.acker(A, B, evdes1)
kr1 = -1/(C * np.linalg.inv(A-B*K1) * B)

Create a system that gives output and control action


In [8]:
sys1 = ctrl.ss(A-B*K1, B*kr1,np.concatenate((C,-K1),axis=0),
              np.concatenate((np.matrix([[0]]),kr1),axis=0))

Plot the reponse for the system


In [9]:
y1, t1 = ctrl.step(sys1,np.linspace(0.,35,1000.))
fig1=create_figure(y1,t1,"Sistema rapido")


Diseno con objetivos mas modestos


In [10]:
zeta0 = zeta0
omega0l = omega0/3
zeta1 = zeta1
omega1l = omega1/2
Tmax = 40
lam3 = -zeta1*omega1l + omega1l*np.sqrt(1-np.power(zeta1,2))*1j
lam4 = -zeta1*omega1l - omega1l*np.sqrt(1-np.power(zeta1,2))*1j
evdes2 = [-zeta0*omega0l + omega0l*1j, -zeta0*omega0l - omega0l*1j, lam3, lam4]

Design the controller and construct the closed loop dynamics


In [11]:
K2 = ctrl.acker(A, B, evdes2)
kr2 = -1/(C * np.linalg.inv(A-B*K2) * B);

Create a system that gives output and control action


In [12]:
sys2 = ctrl.ss(A-B*K2, B*kr2, np.concatenate((C,-K2),axis=0),
          np.concatenate((np.matrix([0]), kr2), axis=0))

y2, t2 = ctrl.step(sys2,np.linspace(0.,35,1000.))
fig2=create_figure(y2,t2,title="Sistema Lento")


SRL-LQR design


In [15]:
G=ctrl.tf(sys)
Gms=G_minus_s(G)
ctrl.rlocus(G*Gms[0],np.logspace(-2,7,500),Plot=True)
[rlist,klist]=ctrl.rlocus(G*Gms[0],[200],Plot=False)
poles_opt=rlist[0][0:4]
K3=ctrl.acker(A,B,poles_opt)
kr3 = -1/(C * np.linalg.inv(A-B*K3) * B);
sys3 = ctrl.ss(A-B*K3, B*kr3, np.concatenate((C,-K3),axis=0),
          np.concatenate((np.matrix([0]), kr3), axis=0))
y3, t3 = ctrl.step(sys3,np.linspace(0.,35,1000.))
fig3=create_figure(y3,t3,title="Sistema Optimo")
#np.linalg.eigvals(A-B*K3)


SRL-LQR design


In [22]:
fig4=plt.figure()
axes4 = fig4.add_axes([0.1, 0.1, 0.8, 0.8])
axes4.plot(t1,y1[:,0],'b--',label='Sistema rapido')
axes4.plot(t2,y2[:,0],'b:',label='Sistema lento')
axes4.plot(t3,y3[:,0],'b-',label='Sistema optimo')
plt.legend(loc=4)

axes4.set_ylabel('Position (m)',color='b')
axes4.set_xlabel('Time(s)')
axes4.grid(which='both')
axes4.set_title("Comparacion")
for tl in axes4.get_yticklabels():
    tl.set_color('b')

axes4i=axes4.twinx()
axes4i.plot(t1,y1[:,1],'r--',label='Sistema rapido')
axes4i.plot(t2,y2[:,1],'r:',label='Sistema lento')
axes4i.plot(t3,y3[:,1],'r-',label='Sistema optimo')
axes4i.set_ylabel('Input Force (N)',color='r',)
axes4i.grid(which='both')
plt.legend(loc=5)
   
for tl in axes4i.get_yticklabels():
    tl.set_color('r')