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.])
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")
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")
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)
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')