Práctica 4 - Control

Para definir sistemas complejos como un robot manipulador, vamos a tener que usar funciones, de tal manera que:

$$ \dot{x} = f(x) $$

como lo hicimos en cada simulación anterior; empezaremos utilizando la de un pendulo simple:



In [ ]:

    
def f(t, x):
    # Se importan funciones matematicas necesarias
    from numpy import cos
    # Se desenvuelven las variables que componen al estado
    q1, q̇1 = x
    # Se definen constantes del sistema
    g = 9.81
    m1, J1 = 0.3, 0.0005
    l1 = 0.4
    τ1 = 0
    # Se calculan las variables a devolver por el sistema
    q1p = q̇1
    q1pp = 1/(m1*l1**2 + J1)*(τ1 - g*m1*l1*cos(q1))
    # Se devuelve la derivada de las variables de entrada
    return [q1p, q1pp]

Para simular utilizaremos la función ode, la cual devolvera un objeto con metodos especiales para simular un sistema como el nuestro:



In [ ]:

    
from scipy.integrate import ode
from numpy import linspace, degrees, radians



In [ ]:

    
sis = ode(f)

Por ejemplo, podemos darle valores iniciales:



In [ ]:

    
sis.set_initial_value([0,0])



In [ ]:

    
ts = linspace(0, 5, 1000)

Y usar una list comprehension para simular el comportamiento del sistema en cada uno de los puntos siguientes:



In [ ]:

    
ys = [sis.integrate(t) for t in ts[1:]]
ys.insert(0,[0,0])

Si ahora queremos graficar el comportamiento del sistema, podemos importar las librerias necesarias:



In [ ]:

    
%matplotlib inline



In [ ]:

    
from matplotlib.pyplot import figure, style
style.use("ggplot")
from matplotlib import rc, rcParams
rcParams['text.latex.preamble']=[r"\usepackage{cmbright}"]

params = {'text.usetex' : True,
          'font.size' : 18}
rcParams.update(params) 

rc('font',**{'family':'sans-serif','sans-serif':['Noto Sans']})

Y graficar:



In [ ]:

    
fig = figure(figsize=(20,5))
ax = fig.gca()
ps = ax.plot(ts, degrees(ys))
ax.legend([ps[0], ps[1]], [r"$q$", r"$\dot{q}$"])
ax.set_title("Sistema sin control");

Sin embargo, este es el comportamiento del sistema sin control, lo que en realidad queremos es que el sistema se mueva a una posición especifica, por lo que necesitamos darle a la función a simular este valor como un parametro extra:



In [ ]:

    
def f(t, x, params):
    qd = params[0]
    # Se importan funciones matematicas necesarias
    from numpy import cos
    # Se desenvuelven las variables que componen al estado
    q1, q̇1 = x
    # Se definen constantes del sistema
    g = 9.81
    m1, J1 = 0.3, 0.0005
    l1 = 0.4
    τ1 = qd
    # Se calculan las variables a devolver por el sistema
    q1p = q̇1
    q1pp = 1/(m1*l1**2 + J1)*(τ1 - g*m1*l1*cos(q1))
    # Se devuelve la derivada de las variables de entrada
    return [q1p, q1pp]

Por el momento utilizare la posición deseada como la función de control

$$ \tau_1 = q_d $$

con lo que al simular y graficar, tenemos:



In [ ]:

    
sis = ode(f)
sis.set_initial_value([0,0])
sis.set_f_params([radians(20)])



In [ ]:

    
ts = linspace(0, 5, 1000)
ys = [sis.integrate(t) for t in ts[1:]]
ys.insert(0,[0,0])



In [ ]:

    
fig = figure(figsize=(20,5))
ax = fig.gca()
ps = ax.plot(ts, degrees(ys))
ax.legend([ps[0], ps[1]], [r"$q$", r"$\dot{q}$"])
ax.set_title("Sistema con control en lazo abierto");

Sin embargo esta ley de control no toma en cuenta la posición real del sistema, podemos definir una ley de control que se calcule con la diferencia de la posición deseada y la posición real:

$$ \tau_1 = q_d - q_1 $$



In [ ]:

    
def f(t, x, params):
    qd = params[0]
    # Se importan funciones matematicas necesarias
    from numpy import cos
    # Se desenvuelven las variables que componen al estado
    q1, q̇1 = x
    # Se definen constantes del sistema
    g = 9.81
    m1, J1 = 0.3, 0.0005
    l1 = 0.4
    τ1 = qd - q1
    # Se calculan las variables a devolver por el sistema
    q1p = q̇1
    q1pp = 1/(m1*l1**2 + J1)*(τ1 - g*m1*l1*cos(q1))
    # Se devuelve la derivada de las variables de entrada
    return [q1p, q1pp]



In [ ]:

    
sis = ode(f)
sis.set_initial_value([0,0])
sis.set_f_params([radians(20)])



In [ ]:

    
ts = linspace(0, 5, 1000)
ys = [sis.integrate(t) for t in ts[1:]]
ys.insert(0,[0,0])



In [ ]:

    
fig = figure(figsize=(20,5))
ax = fig.gca()
ax.plot(ts, degrees(ys))
ax.legend([ps[0], ps[1]], [r"$q$", r"$\dot{q}$"])
ax.set_title(r"Sistema con control $k_p=1$");

Esta ley de control mejoró el comportamiento del sistema (en las primeras 2 simulaciones la amplitud es mayor, cada vez estamos limitando mas el error del sistema), pero aun oscila; el problema por el momento es que si llega a la posición deseada, pero cuando llega a esa posición, su velocidad es mayor que $0$, lo que queremos es que llegue a la posición deseada con velocidad $0$, es decir que llegue a la posición deseada y se quede ahi:

$$ \tau_1 = (q_d - q_1) + (\dot{q}_d - \dot{q}_1) = (q_d - q_1) + (0 - \dot{q}_1) $$



In [ ]:

    
def f(t, x, params):
    qd = params[0]
    # Se importan funciones matematicas necesarias
    from numpy import cos
    # Se desenvuelven las variables que componen al estado
    q1, q̇1 = x
    # Se definen constantes del sistema
    g = 9.81
    m1, J1 = 0.3, 0.0005
    l1 = 0.4
    τ1 = (qd - q1) + (0 - q̇1)
    # Se calculan las variables a devolver por el sistema
    q1p = q̇1
    q1pp = 1/(m1*l1**2 + J1)*(τ1 - g*m1*l1*cos(q1))
    # Se devuelve la derivada de las variables de entrada
    return [q1p, q1pp]



In [ ]:

    
sis = ode(f)
sis.set_initial_value([0,0])
sis.set_f_params([radians(20)])



In [ ]:

    
ts = linspace(0, 5, 1000)
ys = [sis.integrate(t) for t in ts[1:]]
ys.insert(0,[0,0])



In [ ]:

    
fig = figure(figsize=(20,5))
ax = fig.gca()
ax.plot(ts, degrees(ys))
ax.legend([ps[0], ps[1]], [r"$q$", r"$\dot{q}$"])
ax.set_title("Sistema con control $k_p=1$, $k_d=1$");

Este sistema ya empieza a intentar funcionar como debería, sin embargo la posición $q$ no alcanza a llegar a $20^o$ como le dijimos, necesitamos darle una mayor ganacia de control para que intente llegar mas rápido a la posición deseada antes de que la velocidad se vuelva $0$; utilizaremos una ley de control:

$$ \tau_1 = k_p (q_d - q_1) + k_d (0 - \dot{q}_1) $$

en donde $k_p$ y $k_d$ son las ganancias proporcionales y derivativas (corresponden a la posicion y a la velocidad), si les damos valores de $10$ y $2$:



In [ ]:

    
def f(t, x, params):
    qd = params[0]
    # Se importan funciones matematicas necesarias
    from numpy import cos
    # Se desenvuelven las variables que componen al estado
    q1, q̇1 = x
    # Se definen constantes del sistema
    g = 9.81
    m1, J1 = 0.3, 0.0005
    l1 = 0.4
    kp, kd = 10, 2
    τ1 = kp*(qd - q1) + kd*(0 - q̇1)
    # Se calculan las variables a devolver por el sistema
    q1p = q̇1
    q1pp = 1/(m1*l1**2 + J1)*(τ1 - g*m1*l1*cos(q1))
    # Se devuelve la derivada de las variables de entrada
    return [q1p, q1pp]



In [ ]:

    
sis = ode(f)
sis.set_initial_value([0,0])
sis.set_f_params([radians(20)])



In [ ]:

    
ts = linspace(0, 5, 1000)
ys = [sis.integrate(t) for t in ts[1:]]
ys.insert(0,[0,0])



In [ ]:

    
fig = figure(figsize=(20,5))
ax = fig.gca()
ax.plot(ts, degrees(ys))
ax.legend([ps[0], ps[1]], [r"$q$", r"$\dot{q}$"])
ax.set_title("Sistema con control $k_p=10$, $k_d=2$");

En este punto tenemos un sistema estable, el cual casi llega a la posición deseada, tan solo tenemos que sintonizar las ganancias $k_p$ y $k_d$. Para este procedimiento tenemos que modificar el valor de $k_p$ y $k_d$, por prueba y error, para mejorar el comportamiento del sistema. Sin embargo, con la ayuda de Jupyter podemos hacer algo mejor... primero defino la función de tal manera que los parametros de entrada me den no solo $q_d$, sino $k_p$ y $k_d$ tambien:



In [ ]:

    
def f(t, x, params):
    qd, kp, kd = params
    # Se importan funciones matematicas necesarias
    from numpy import cos
    # Se desenvuelven las variables que componen al estado
    q1, q̇1 = x
    # Se definen constantes del sistema
    g = 9.81
    m1, J1 = 0.3, 0.0005
    l1 = 0.4
    τ1 = kp*(qd - q1) + kd*(0 - q̇1)
    # Se calculan las variables a devolver por el sistema
    q1p = q̇1
    q1pp = 1/(m1*l1**2 + J1)*(τ1 - g*m1*l1*cos(q1))
    # Se devuelve la derivada de las variables de entrada
    return [q1p, q1pp]

Despues, definimos una función que va a simular el comportamiento del sistema, usando como variables de entrada los valores que queremos variar:



In [ ]:

    
def sim_sist_cont(qd=0, kp=1, kd=1):
    sis = ode(f)
    sis.set_initial_value([0,0])
    sis.set_f_params([radians(qd), kp, kd])
    ts = linspace(0, 1, 100)
    ys = [sis.integrate(t) for t in ts[1:]]
    ys.insert(0,[0,0])
    fig = figure(figsize=(20,5))
    ax = fig.gca()
    ax.plot(ts, degrees(ys))
    ax.set_ylim(-200, 200)
    ax.legend([ps[0], ps[1]], [r"$q$", r"$\dot{q}$"])
    ax.set_title("Sistema con control $k_p="+str(kp)+"$, $k_d="+str(kd)+"$");

Una vez que tenemos esto, importamos interact para modificar los valores interactivamente:



In [ ]:

    
from ipywidgets import interact



In [ ]:

    
interact(sim_sist_cont, qd=(-180.0, 180.0), kp=(0,100.0), kd=(0,5.0))

Ejercicio

Utiliza esta visualización interactiva para sintonizar el sistema controlado, de tal manera que el tiempo de estabilización sea menor a $0.2s$ y el sobrepaso máximo sea menor a $5\%$.

Define las variables kp y kd con los resultados de esta sintonización:



In [ ]:

    
# ESCRIBE TU CODIGO AQUI
raise NotImplementedError



In [ ]:

    
from nose.tools import assert_almost_equal
def prueba_sist_cont(qd, kp, kd):
    sis = ode(f)
    sis.set_initial_value([0,0])
    sis.set_f_params([radians(qd), kp, kd])
    ts = linspace(0, 1, 100)
    ys = [sis.integrate(t) for t in ts[1:]]
    ys.insert(0,[0,0])
    return ts, ys

ts, ys = prueba_sist_cont(120, kp, kd)
ps, vs = zip(*ys)
ve = ps[-1]
sm = max([p-ve for p in ps])
for i in range(len(ps)-1):
    if ps[i+1] - ps[i] < 0.01:
        te = ts[i]
        break

assert 0 < te <= 0.2, "El tiempo de estabilización es mayor a 0.2s"
assert 0 < sm <= 0.05*radians(180), "El sobrepaso maximo es mayor al 5%"

Una vez que terminamos esto, podemos pasar adelante al documento simulador.ipynb