# Modelo simplificado de la dinámica lateral

## Modelo dinámico

La dinámica de inclinación de una moto se puede aproximar al comportamiento de un péndulo invertido, si consideramos el punto de contacto de la rueda con el suelo $O$ como una articulación con el eje paralelo a la dirección de la marcha. La siguiente figura muestra un esquema del modelo, que se puede interpretar como una vista de la moto desde atrás, donde $G$ es el centro de masas:



Durante el movimiento de rodadura, el punto de contacto entre la rueda y el suelo corresponderá en cada instante a puntos materiales diferentes, tanto de la rueda como del suelo. Si la rodadura se produce sin deslizamiento, dicho punto de contacto tendrá siempre velocidad nula, por lo que constituirá el *centro instantáneo de rotación* o CIR de la rueda. Aunque el punto de contacto físico tenga velocidad nula en cada instante, la localización del contacto irá cambiando a lo largo del tiempo, siguiendo una trayectoria sobre el suelo. La velocidad a la que se produce este desplazamiento del punto de contacto se denomina *velocidad de sucesión*, y es fácil demostrar que, cuando hay rodadura sin deslizamiento y el desplazamiento es rectilíneo, coincide con la velocidad de traslación del centro de la rueda o, lo que es lo mismo, con la velocidad de avance de la moto $v$. Si el movimiento no es rectilíneo pero el radio de curvatura $\rho$ de la trayectoria es suficientemente grande, $v$ sigue siendo una buena estimación de la velocidad de sucesión.

En nuestro modelo, por lo tanto, vamos a considerar que la articulación $O$ se mueve sobre el suelo a una velocidad $v$ (que consideraremos constante para simplificar el estudio), a lo largo de una trayectoria curvilínea con radio de curvatura $\rho$, que dependerá del ángulo de dirección $u$ y de la distancia entre ejes de la moto $l$, como se verá a continuación.

## Efecto del ángulo de dirección

Para estimar la relación entre el ángulo de dirección $u$ y el radio de curvatura de la trayectoria $\rho$, consideraremos un modelo simplificado de la moto como el que se muestra en la figura, en este caso visto desde arriba:



En este modelo, a diferencia de lo que ocurre en una moto real, el eje de dirección es vertical y pasa por el centro de la rueda delantera. Por este motivo, cuando la moto no está inclinada, la distancia entre ejes $l$ permanecerá constante al girar la dirección un ángulo cualquiera $u$. En estas condiciones, para cumplir el campo de velocidades de sólido rígido del chasis sin que las ruedas tengan deslizamiento, los centros de curvatura de sus trayectorias deben coincidir en un mismo punto, que será a su vez el CIR del chasis. Se demuestra fácilmente que este punto es la intersección de los ejes de ambas ruedas, como muestra la figura anterior.

De este modo, se puede establecer la relación geométrica entre el ángulo de dirección y los radios de curvatura $\rho_f$ y $\rho_r$ de las trayectorias de las ruedas delantera y trasera, respectivamente:

$$
    \rho_f = \frac{l}{\sin u} \qquad \rho_r = \frac{l}{\tan u}
$$

Para ángulos de dirección $u$ pequeños, ambos radios se pueden aproximar a un mismo valor $\rho$:

$$
    \rho \approx \frac{l}{u}
$$

## Ecuación del movimiento

Para estudiar el movimiento del péndulo, se consideran unos ejes móviles $\bar x \bar y \bar z$ con origen en $O$, tales que $\bar y$ apunta siempre en la dirección de la marcha (es decir, es tangente a la trayectoria) y $\bar z$ apunta siempre hacia arriba, coincidiendo con el eje global $z$. Para que los ejes locales se mantengan siempre tangentes a la trayectoria, deberán tener una velocidad angular $\mathbf\Omega$:

$$
    \mathbf \Omega = \begin{bmatrix} 0 \\ 0 \\ \Omega \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \\ \frac{v}{\rho} \end{bmatrix}
$$

que, al ser en dirección $\bar z$, coincidirá con su expresión en ejes locales $\bar{\mathbf\Omega}$. Consideraremos que el radio de curvatura $r$ es positivo cuando la moto gira hacia la izquierda (velocidad angular positiva sobre el eje $z$). La velocidad angular del péndulo, expresada en los ejes locales $\bar x \bar y \bar z$, será el resultado de sumar esta velocidad angular de arrastre $\bar{\mathbf\Omega}$, a una velocidad angular relativa según el eje $\bar y$, causada por las variaciones del ángulo $\varphi$:

$$
    \bar{\mathbf\omega} = \begin{bmatrix} 0 \\ \dot\varphi \\ \frac{v}{\rho} \end{bmatrix}
$$

Para obtener la aceleración angular, hay que derivar este vector en la base móvil usando la fórmula de Boure:

$$
    \dot{\bar{\mathbf\omega}} = \left.\frac{d{\bar{\mathbf\omega}}}{dt}\right|_m + \bar{\mathbf\Omega}\times\bar{\mathbf\omega} = \begin{bmatrix} -\frac{v}{\rho}\dot\varphi \\ \ddot\varphi \\ 0 \end{bmatrix}
$$

En esta derivación, se considera que el radio de curvatura es constante, ya que técnicamente no es un grado de libertad del movimiento (no se puede variar arbitrariamente, depende de la trayectoria). Aplicando el campo de aceleraciones del sólido rígido, la aceleración del centro de masas en ejes locales $\bar{\mathbf a}_G$ será igual a:

$$
    \bar{\mathbf a}_G = \bar{\mathbf a}_O + \bar{\mathbf\omega}\times\left(\bar{\mathbf\omega}\times\bar{\mathbf{OG}}\right) + \dot{\bar{\mathbf\omega}}\times\bar{\mathbf{OG}}
$$

donde donde $\bar{\mathbf a}_O$ es la aceleración del punto de contacto $O$, que será la que le corresponde por tener una trayectoria curvilínea de radio $r$ a velocidad constante $v$:

$$
    \bar{\mathbf a}_O = \begin{bmatrix} -\frac{v^2}{\rho} \\ 0 \\ 0 \end{bmatrix}
$$

Sustituyendo esto en la expresión anterior y evaluando los productos vectoriales, se obtiene la aceleración del centro de masas en ejes locales:

$$
    \bar{\mathbf a}_G = \begin{bmatrix} -\frac{v^2}{\rho^2}\left(\rho + h\sin\varphi\right) - h\dot\varphi^2\sin\varphi + h\ddot\varphi\cos\varphi \\
                 2h\frac{v}{\rho}\dot\varphi\cos\varphi \\ - h\dot\varphi^2\cos\varphi - h\ddot\varphi\sin\varphi \end{bmatrix}
$$

Ahora que ya tenemos la aceleración del centro de masas, podemos plantear las ecuaciones de Newton-Euler para nuestro sistema, considerando únicamente la dinámica en el plano $\bar x\bar z$. Según la primera figura, donde se muestran las componentes de la aceleración de $G$ en azul, se puede ver que:

$$
    \begin{align}
        & R_x = m\bar a_{Gx} \\
        & R_z - mg = m\bar a_{Gz} \\
        & R_zh\sin\varphi - R_xh\cos\varphi = I_G\ddot\varphi
    \end{align}
$$

Estas ecuaciones son no lineales, así que hay que linealizarlas alrededor de la posición de equilibrio ($\varphi = 0$) para poder expresar la dinámica del sistema en forma *state-space*. Si se consideran valores pequeños de $\varphi$ y $\dot\varphi$, se puede asumir que $\sin\varphi \approx \varphi$, $\cos\varphi \approx 1$, $\varphi^2 \approx 0$ y $\dot\varphi^2 \approx 0$. Además, si $\rho$ es grande, podemos considerar que $\rho^2$ tiende a infinito. Aplicando estas simplificaciones, y sustituyendo los valores de $R_x$ y $R_z$ de las dos primeras ecuaciones en la tercera, se obtiene la ecuación del movimiento linealizada:

$$
    \left(I_G + mh^2\right)\ddot\varphi = mgh\varphi + mh\frac{v^2}{\rho}
$$

Sustituyendo la expresión de $\rho$ en función de $u$, se obtiene la ecuación que se utilizará para diseñar el controlador:

$$
    \left(I_G + mh^2\right)\ddot\varphi = mgh\varphi + mh\frac{v^2}{l}u
$$

Entonces, los parámetros que vamos a necesitar para diseñar el controlador son la masa total $m$, el momento de inercia respecto a un eje longitudinal que pasa por el centro de masas $I_G$, la altura del centro de masas $h$ y la distancia entre ejes $l$, además de la velocidad de avance $v$.

## Sistema linealizado en forma *state-space*

Ya tenemos la ecuación del movimiento linealizada alrededor de la posición de equilibrio, en forma de ecuación diferencial lineal con coeficientes constantes. Para poder diseñar el controlador, queremos reescribirla usando la representación *state-space*, que para un sistema SISO  (*single input, single output*) genérico, con una entrada $u$ y una salida $y$, tiene la forma:

$$
    \begin{align}
        & \dot{\mathbf x}=\mathbf{Ax}+\mathbf{B}u \\
        & y = \mathbf{Cx} + Du
    \end{align}
$$

La primera ecuación describe la dinámica del sistema, que se modeliza a través de una serie de *estados* contenidos en un vector $\mathbf x$, que son controlados por medio de una entrada o *input* $u$ (en nuestro caso, el ángulo de dirección). La matriz $\mathbf A$ y el vector $\mathbf B$ son constantes, de modo que se trata de un sistema de ecuaciones diferenciales lineales con coeficientes constantes. La segunda ecuación relaciona la salida o *output* $y$, es decir, la magnitud del sistema que queremos conocer y/o controlar (el ángulo de inclinación $\varphi$ en nuestro sistema), con los estados $\mathbf x$ y el *input* $u$, a través de una matriz $\mathbf C$ y un escalar $D$, también constantes.

La ecuación del movimiento de nuestro sistema es de segundo orden, así que tendremos que establecer el vector de estados de forma que se pueda expresar como un sistema de primer orden. Podemos elegir los estados $\mathbf x$ de la siguiente manera:

$$
    \mathbf{x}=
    \begin{bmatrix}
        \varphi \\ \dot\varphi
    \end{bmatrix}
    \implies \dot{\mathbf{x}}=
    \begin{bmatrix}
        \dot\varphi \\ \ddot\varphi
    \end{bmatrix}
$$

Esto nos permite reescribir la ecuación del movimiento en la forma $\dot{\mathbf x}=\mathbf{Ax}+\mathbf{B}u$:

$$
    \begin{bmatrix}
        \dot\varphi \\ \ddot\varphi
    \end{bmatrix} =
    \begin{bmatrix}
        0 && 1 \\ \frac{mgh}{\left(I_G+mh^2\right)} && 0
    \end{bmatrix}
    \begin{bmatrix}
        \varphi \\ \dot\varphi
    \end{bmatrix} + 
    \begin{bmatrix}
        0 \\ \frac{mhv^2}{l\left(I_G+mh^2\right)}
    \end{bmatrix}u
$$

Como se mencionó más arriba, la salida del sistema $y$ es el propio ángulo de inclinación $\varphi$, por lo que los cuatro términos $\mathbf A$, $\mathbf B$, $\mathbf C$ y $D$ resultan finalmente:

$$
    \mathbf A = \begin{bmatrix} 0 && 1 \\ \frac{mgh}{\left(I_G+mh^2\right)} && 0 \end{bmatrix} \qquad
    \mathbf B = \begin{bmatrix} 0 \\ \frac{mhv^2}{l\left(I_G+mh^2\right)} \end{bmatrix} \qquad
    \mathbf C = \begin{bmatrix} 1 && 0 \end{bmatrix} \qquad
    D = 0
$$

Para poder continuar con el diseño del controlador, se asignarán valores numéricos a los parámetros. En este caso se utilizarán unos valores genéricos, cuando se tenga la moto construida habrá que estimar los valores reales y sustituirlos. Cada vez que se modifique una celda de código, hay que volver a ejecutarla usando SHIFT+ENTER. Además, habrá que ejecutar también todas las que haya debajo, o al menos las que se vean afectadas por los cambios. Se pueden usar las opciones que hay en el menú de Jupyter, para ejecutar una celda y las siguientes, o todas desde el principio, etc.


```python
import numpy as np

m = 100.0 # Masa en kg
I = 10.0  # Momento de inercia en kg·m2
v = 10.0  # Velocidad en m/s
l = 1.0   # Distancia entre ejes en m
h = 1.0   # Altura del CDG en m
g = 9.81  # Aceleración de la gravedad en m/s2

A = np.array([[0.0, 1.0], [m*g*h/(I + m*h**2), 0.0]])
B = np.array([[0], [m*h*v**2/l/(I + m*h**2)]])
C = np.array([[1.0, 0.0]])
D = np.array([[0]])

# Mostramos en la salida el contenido de A y B
print(A)
print(B)
```

# Diseño del controlador

## Estabilidad del sistema

En la bibliografía sobre Control es frecuente referirse al sistema que queremos controlar, de modo general, como *planta*. Cuando no hay realimentación, el sistema se conoce como *planta en lazo abierto*. Si consideramos el sistema libre, sin introducir ningún *input*, resultará una ecuación diferencial de la forma:

$$
    \dot{\mathbf x} = \mathbf A\mathbf x
$$

Se dice que un sistema es *asintóticamente estable* si, para unas condiciones iniciales cualesquiera, el vector de estados $\mathbf x$ tiende a cero cuando el tiempo tiende a infinito. Se puede demostrar que la solución de un sistema de este tipo es estable cuando todos los autovalores de $\mathbf A$ tienen parte real negativa. Como se puede ver, nuestro sistema no es estable, ya que tiene un autovalor con parte real positiva:


```python
# Comprobamos los autovalores de 'A'
print(np.real(np.linalg.eig(A)[0]))
```

## Primera aproximación: regulador PID

Cuando no se dispone de un modelo dinámico del sistema, la forma más simple de estabilizarlo es utilizando un regulador PID. En este caso, se podría utilizar un PID en el que la entrada fuese el error entre el ángulo de inclinación deseado y el real, y la salida el ángulo de dirección. En general, lo que hay que hacer para estabilizar una moto es girar el manillar en la dirección de la caída ([*steer into the fall*](https://youtu.be/o7nSQ2ycGX4)).

Para eso, es necesario tener una estimación del ángulo de la moto, y la aproximación más simple es utilizar el ángulo de equilibrio estático, es decir, el que mantiene la moto en equilibrio al girar con velocidad y radio constantes. Este ángulo se puede estimar a partir de la ecuación del movimiento, haciendo nulas las derivadas del ángulo $\varphi$:

$$
    mgh\varphi + mh\frac{v^2}{\rho} = 0 \implies \varphi \approx - \frac{v^2}{\rho g} = -\frac{v}{\rho}\frac{v}{g} = -\Omega\frac{v}{g}
$$

donde $\Omega$, que como recordaremos es la velocidad de rotación del sistema de referencia $\bar x\bar y\bar z$, se puede aproximar para inclinaciones pequeñas al valor proporcionado por el eje vertical del giróscopo, mientras que $v$ se puede estimar colocando un *encoder* en la rueda trasera.

El problema de utilizar este tipo de controladores es que suelen ser difíciles de ajustar, por lo que, si es posible, es mejor recurrir a métodos más avanzados, como la realimentación de estados, que se describe a continuación.


## Estabilización mediante realimentación de estados

La realimentación de estados se basa en definir la entrada del sistema $u$ como una realimentación negativa del estado, de la siguiente forma:

$$
    u = -\mathbf K\mathbf x
$$

Con este tipo de realimentación, se puede determinar de forma analítica una matriz de ganancias $\mathbf K$ que estabilice el sistema, es decir, que haga que todos los autovalores sean negativos. Para verlo más claramente, se puede introducir la realimentación en las ecuaciones del sistema:

$$
    \dot{\mathbf x} = \mathbf{Ax} - \mathbf{BKx} = \left(\mathbf A - \mathbf{BK}\right)\mathbf x
$$

Se observa que, ahora, la estabilidad del sistema la determinan los autovalores de $\mathbf A - \mathbf{BK}$. Esto significa que se puede alterar la dinámica del sistema a conveniencia, ajustando la matriz $\mathbf K$ para colocar los autovalores donde nos interese. Este método de control se puede esquematizar de la forma siguiente:



## Regulador cuadrático lineal (LQR)

El esquema de control de realimentación de estados deja abierta una decisión complicada al diseñador: ¿dónde colocar los autovalores del sistema? Existen diversos métodos descritos en la bibliografía, dedicados a determinar dónde se deben colocar los autovalores para conseguir que la respuesta del sistema presente unas características determinadas. Estas técnicas se denominan *asignación de polos* o *pole placement*.

En nuestro proyecto, vamos a utilizar un método un poco más elaborado, pero a la vez sencillo de utilizar: el LQR (*Linear Quadratic Regulator*). El LQR no es más que otro método para calcular la matriz $\mathbf K$, pero en lugar de tener que decidir dónde colocamos los polos del sistema, lo que hacemos es buscar el valor único de $\mathbf K$ que minimiza la siguiente función:

$$
    J = \int_0^\infty\left(\mathbf x^T\mathbf Q\mathbf x + Ru^2\right)dt
$$

Esta función (expresada aquí en forma específica para un sistema con un solo *inupt*) combina los valores cuadráticos, integrados a lo largo del tiempo, de las dos magnitudes que queremos minimizar: el error en los estados y la magnitud de la entrada de control. Cada término va ponderado por una matriz de pesos, $\mathbf Q$ para los estados y $R$ para el *input*, de modo que podemos decidir qué variables queremos penalizar más en el controlador. Por ejemplo, asignar un valor elevado al elemento $Q_{ii}$ de la diagonal de $\mathbf Q$ hará que los errores en el estado $x_i$ sean fuertemente penalizados en la función objetivo $J$, así que el controlador se esforzará en manternerlos bajos. Por el contrario, si se hace lo mismo con $R$, se indica al controlador que debe utilizar lo menos posible el actuador correspondiente al *input* $u$.

Modificando los elementos de $\mathbf Q$ y $R$ se puede ajustar el comportamiento del controlador a nuestros requerimientos, dando mayor relevancia al error en los estados o al esfuerzo de control. Lo primero puede ser importante si en algún estado es crítico mantener errores bajos, y lo segundo, por ejemplo, si es prioritario obtener una alta eficiencia energética, aunque sea al coste de menor precisión en el control. En nuestro caso, se podría utilizar para evitar que el controlador use ángulos de dirección demasiado grandes.

Una vez dererminadas las matrices de pesos, la función `lqr` del módulo de control de Python resuelve el problema de optimización y nos devuelve directamente la matriz de ganancias $\mathbf K$ que minimiza la función $J$:


```python
from control import ss, lqr, ss2tf, minreal, forced_response

sys = ss(A, B, C, D)

Q = np.eye(2) # Aproximación incial para 'Q'
Q[0, 0] = 10  # Ajustamos peso del ángulo 'phi'
R = 1         # Peso del ángulo de dirección 'u'

K = lqr(sys, Q, R)[0]
```

Para evaluar el comportamiento del regulador, se simula el comportamiento del sistema cuando partimos de un estado inicial fuera de equilibrio. Por ejemplo, colocamos la moto con un ángulo inicial $\varphi_0$ de 20º, mientras circula en línea recta a la velocidad prefijada $v$ (10 m/s), de modo que el controlador generará un movimiento de dirección que la estabilice en vertical.

La entrada de referencia $r$ que se ve en el diagrama del controlador se dejará a cero (se explicará más adelante para qué sirve esta entrada). Como ejercicio, es interesante comprobar cómo varía la repuesta al modificar los pesos del LQR:


```python
%%capture
from matplotlib.animation import FuncAnimation
import plotfunctions as pf
```


```python
# Ensamblamos el sistema en lazo cerrado
clsys = ss(A - B*K, B, C, D)

tf = 4.00         # Duración de la simulación en segundos
dt = 0.01         # Paso de tiempo de simulación en segundos

# Simulamos respuesta para ángulo incial de 20 grados
t = np.arange(0, tf, dt)
r = np.zeros(t.shape)

T, Y, X = forced_response(clsys, t, r, X0=[np.radians(20), 0])

pf.plot_response(T, X)
```

Como se ve, el ángulo $\varphi$ converge rápidamente a cero, estabilizando el sistema.

Podemos verificar que, al aplicar la realimentación, todos los autovalores del sistema tienen ahora parte real negativa:


```python
print(np.real(np.linalg.eig(A - B*K)[0]))
```

A continuación se muestra una animación con el movimiento resultante:


```python
FuncAnimation(pf.fg1, pf.anim, init_func=pf.init, frames=len(T), fargs=(X[[0, 1]], dt, h), interval=dt*1000, blit=True)
```

## Control de la inclinación

Cuando se aplica realimentación de estados, el controlador resultante siempre tratará de llevarlos todos al equilibrio, y eso es lo que se conoce como *regulador*. Hasta ahora, esto nos ha servido para estabilizar el sistema, manteniéndolo equilibrado en vertical. Si lo que queremos es asignar a algún *output* un valor arbitrario, hay que convertir nuestro regulador en un *servomecanismo*. El objetivo ahora es conseguir que el *output* (en nuestro caso el ángulo de inclinación $\varphi$) siga a un valor de referencia $r$, en lugar de converger hacia cero. De este modo, podremos controlar la dirección de la moto sin perder la estabilidad, ya que controlar el ángulo de inclinación implica controlar indirectamente el radio de curvatura de la trayectoria.

Antes de explicar cómo se consigue esto, es importante introducir un nuevo concepto: el *tipo* de sistema o planta. Por definición, una planta es de tipo $N$ cuando, al representarla como un sistema con realimentación unitaria (como el que se muestra en la figura), el término de grado más bajo del denominador de la función de transferencia $G$ es de grado $N$.



Es decir, si la función de transferencia en lazo abierto $G$ se escribe en la forma:

$$
    G(s) = \frac{K\left(T_as + 1\right)\left(T_bs + 1\right)\cdots\left(T_ms + 1\right)}
    {s^N\left(T_1s + 1\right)\left(T_2s + 1\right)\cdots\left(T_ps + 1\right)}
$$

el tipo de sistema corresponderá al exponente $N$ que aparece en el denominador. El tipo de un sistema determina su error de seguimiento $e$ en régimen permanente para distintos tipos de entrada. Por ejemplo, en un sistema tipo 1, el error tenderá a cero para una entrada escalón, a un valor finito para una entrada rampa, y a infinito para una entrada cuadrática o de orden superior. En cambio, un sistema tipo 0 ya mostrará un error finito en régimen permanente para una entrada escalón, y será inestable frente a una entrada rampa o superior.

Para saber el tipo de nuestro sistema, primero tenemos que representarlo según la estructura de la figura. La función de transferencia $H$ del sistema completo, entre la entrada $r$ y la salida $y$, se puede obtener sustituyendo el error de seguimiento $e$ por su valor $r - y$:

$$
    y = G\left(r - y\right) \implies H(s) = \frac{y}{r} = \frac{G}{1 + G}
$$

Entonces, se puede hacer el cambio inverso para expresar cualquier sistema con función de transferencia $H$ como sistema con realimentación unitaria:

$$
    G(s) = \frac{H}{1 - H}
$$

Comprobamos el tipo de nuestro sistema, incluyendo la realimentación, cuando consideramos que nuestro único *output* es la inclinación. La función `ss2tf` devuelve la función de transferencia $H$ de un sistema *state-space*, y `minreal` (*minimal realization*) cancela los factores comunes del numerador y el denominador:


```python
# Función de transferencia del sistema con realimentación
H = ss2tf(clsys)

# Calculamos G para transformar en sistema con realimentación unitaria
minreal(H/(1 - H))
```

Mirando el denominador, se ve claramente que nuestro sistema realimentado es de tipo 0. Eso significa que, para una entrada escalón, la salida $\varphi$ nunca alcanzará el valor de referencia $r$ en régimen permanente. Si queremos controlar la inclinación correctamente, tendremos que convertir el sistema en uno de tipo 1.

## Servomecanismo tipo 1 para controlar la inclinación

Existen varios métodos para convertir un sistema de tipo 0 en uno de tipo 1. Lo más sencillo es escalar la entrada de referencia $r$ mediante una ganancia de precompensación $\bar N$, que haga que el *output* que queremos controlar (en nuestro caso la inclinación $\varphi$) alcance el valor solicitado en régimen permanente. En la figura se muestra un diagrama del sistema original, con la precompensación aplicada en la entrada:



Al añadir la precompensación, la entrada del sistema en lazo abierto pasa a ser:

$$
    u = -\mathbf{Kx} + \bar Nr
$$

Vamos a estudiar lo que ocurre con $\mathbf x$ y $u$ cuando tenemos una entrada de referencia $r$ de tipo escalón unitario. Supondremos que, en régimen permanente, la salida $y$ alcanza efectivamente el valor de referencia (es decir, la unidad). Como en régimen estacionario las derivadas de los estados $\dot{\mathbf x}$ serán nulas, podemos escribir:

$$
    \begin{bmatrix} \mathbf 0 \\ 1 \end{bmatrix} = 
    \begin{bmatrix} \mathbf A && \mathbf B \\ \mathbf C && D \end{bmatrix}
    \begin{bmatrix} \mathbf{x_\infty} \\ u_\infty \end{bmatrix}
$$

Si la matriz de este sistema lineal es invertible, resolviéndolo obtendremos el valor de $\mathbf x_\infty$ y $u_\infty$. Como la referencia $r$ es un escalón unitario, la entrada del sistema en lazo abierto alcanzará el siguiente valor:

$$
    u_\infty = -\mathbf{Kx}_\infty + \bar N
$$

de donde obtenemos $\bar N$ como:

$$
    \bar N = \mathbf{Kx}_\infty + u_\infty
$$


```python
# Precompensación para salida 'xp' en régimen permanente
xuinf = np.linalg.solve(np.block([[A, B], [C, D]]), [0, 0, 1])
Nb = K@xuinf[:-1] + xuinf[-1]
```

Antes de simular la respuesta, podemos montar el sistema en lazo cerrado con precompensación en la entrada, y comprobar su tipo:


```python
# Sistema en lazo cerrado, incluyendo la precompensación
clsyspc = ss(A - B*K, B*Nb, C, D)

# Función de transferencia del sistema con realimentación
H = ss2tf(clsyspc)

# Calculamos G(s) para sistema con realimentación unitaria
minreal(H/(1 - H))
```

Vemos que el sistema se puede considerar prácticamente de tipo 1, ya que el término de grado 0 del denominador es cero o despreciable (varía según la plataforma donde se ejecute el código).

Para evaluar el comportamiento del controlador, se simulará una maniobra simple, equivalente a tomar una curva hacia la derecha. Partiendo del equilibrio en línea recta, se pasará el ángulo deseado de 0 a 20 grados, se mantendrá así durante dos segundos, y se volverá a poner a cero después.


```python
# Simulamos respuesta a escalón
t = np.arange(0, tf, dt)
r = np.zeros(len(t))
r[t < 2] = np.radians(20)

T, Y, X = forced_response(clsyspc, t, r)

pf.plot_response(T, X)
```

Se observa que el sistema alcanza efectivamente el ángulo objetivo, y vuelve a la vertical de forma controlada.

Antes de implementar este controlador, hay que tener cuidado con los valores del *input*, ya que al estar el modelo linealizado, pueden tomar cualquier valor. Esto quiere decir que nos pueden salir ángulos de dirección poco realistas, como 120 grados. Es importante entonces observar los ángulos que está utilizando el controlador:


```python
pf.plot_steering(T, (Nb*r - K@X).T)
```

Vemos que, para alcanzar el ángulo solicitado de 20 grados ($\varphi$ positivo, hacia la derecha), la dirección gira brevemente 60 grados ($u$ positivo, hacia la izquierda), y cuando alcanza la inclinación deseada se mantiene con un ligero ángulo hacia la derecha. Después, cuando se le pide volver a la vertical al cabo de 2 segundos, da un toque aún más hacia la derecha, que hace que la moto se vaya dirigiendo de nuevo a la vertical. Esto es bien conocido por cualquiera que haya montado en moto, y es lo que se conoce como *contramanillar*: para girar hacia un lado, antes hay que dar un toque al manillar hacia el lado contrario, lo que ayuda a inclinar la moto en la dirección buscada.

El problema es que un giro de 60 grados se sale de la zona de validez del modelo lineal, aparte de que un golpe de manillar de esa magnitud en una moto que circula a 10 m/s no parece muy razonable. Para corregir esto, hay que reajustar los pesos en el LQR, aumentando el peso del esfuerzo de control $\mathbf R$ para penalizar el uso de ángulos grandes, con la contrapartida de que la maniobra será más lenta. Es interesante probar diferentes valores, volviendo a ejecutar el código para ver cómo varía la respuesta.

A continuación se muestra una animación del movimiento obtenido:


```python
FuncAnimation(pf.fg1, pf.anim, init_func=pf.init, frames=len(T), fargs=(X[[0, 1]], dt, h), interval=dt*1000, blit=True)
```

Los parámetros obtenidos ya sirven para controlar la inclinación de la moto. Los valores mostrados más abajo se pueden copiar y pegar directamente en un *sketch* de Arduino, de forma que, para implementar el controlador, sólo habría que calcular el ángulo de dirección como $u = -\mathbf{Kx} + \bar Nr$.


```python
print('const float K1 =%10.6f;  // Ganancia phi'    % K[0, 0])
print('const float K2 =%10.6f;  // Ganancia phip'   % K[0, 1])
print('const float Nb =%10.6f;  // Precompensación' % Nb)
```

Una de las principales desventajas de los métodos de realimentación de estados es que, para calcular $u$, necesitamos conocer el valor de todos los estados (además de los parámetros del sistema). Esto es un problema, porque normalmente no todos los estados se pueden medir fácilmente con precisión. En estos casos, lo que se hace es utilizar un *observador*, que consiste en un modelo del sistema que se va integrando en el tiempo, corrigiendo los errores de integración a partir de los datos de los sensores.

Como el uso de observadores embebidos en el controlador queda fuera del ámbito de este proyecto, es usará una estimación de estados simplificada. En nuestro sistema, el vector $\mathbf x$ contiene dos estados: $\varphi$ y $\dot\varphi$. El primero se puede estimar usando el método descrito más arriba, utilizando las condiciones de equilibrio estático. El segundo es muy fácil de medir, ya que nos lo proporciona directamente la lectura del eje longitudinal del giróscopo. Si esto no resultase válido, se podría plantear el uso de un observador más avanzado.

## Servomecanismo tipo 1 con integrador en la entrada

El problema de usar precompensación es que la respuesta es muy sensible a errores en los parámetros del sistema. Cualquier desviación en el valor de $\bar N$ hará que el sistema vuelva a ser de tipo 0, y por lo tanto tenga error en régimen permanente. La manera de resolver esto de forma más robusta es añadir a la entrada un integrador, en lugar de una simple ganancia:



Para utilizar este esquema de control, hay que aumentar el sistema añadiéndole un estado adicional $\xi$, que será la integral del error de seguimiento $r - y$. Por lo tanto, su derivada es:

$$
    \dot\xi = r - y = r - \left(\mathbf{Cx} + Du\right)
$$

y la entrada $u$ pasa a ser ahora:

$$
    u = -\mathbf K \mathbf x + k_I\xi
$$

La ecuación completa de la dinámica del sistema aumentado, sin incluir la realimentación, se puede escribir como:

$$
    \begin{bmatrix} \dot{\mathbf x} \\ \dot\xi \end{bmatrix} = 
    \begin{bmatrix} \mathbf A && \mathbf 0 \\ -\mathbf C && 0 \end{bmatrix}
    \begin{bmatrix} \mathbf{x} \\ \xi\end{bmatrix} + 
    \begin{bmatrix} \mathbf B \\ -D \end{bmatrix}u + 
    \begin{bmatrix} \mathbf 0 \\ 1 \end{bmatrix}r
$$

Estudiaremos la respuesta de este sistema a una entrada escalón, cuando el tiempo tiende a infinito. Para ello, definiremos las siguientes magnitudes:

$$
    \mathbf e = \begin{bmatrix} \mathbf{x} - \mathbf{x}_\infty \\ \xi - \xi_\infty \end{bmatrix} \qquad
    u_e = u - u_\infty
$$

que representan las desviaciones del estado y la entrada respecto sus propios valores en régimen permanente. Como $r$ es un escalón, su valor es siempre el mismo para $t>0$, de forma que podemos escribir la dinámica del error como:

$$
    \dot{\mathbf e} = 
    \begin{bmatrix} \mathbf A && \mathbf 0 \\ -\mathbf C && 0 \end{bmatrix}
    \mathbf e + 
    \begin{bmatrix} \mathbf B \\ -D \end{bmatrix}u_e = \hat{\mathbf A}\mathbf e + \hat{\mathbf B}u_e
$$

Se puede observar que, de forma análoga a lo que ocurría al principio, tenemos un sistema en forma *state-space* cuyos estados tienen que converger asintóticamente a cero. Eso quiere decir que existirán unas ganancias de realimentación $\hat{\mathbf K}$ que estabilicen el sistema:

$$
    \hat{\mathbf K} = \begin{bmatrix} \mathbf K && -k_I \end{bmatrix}
$$

Estas ganancias se pueden calcular, como antes, utilizando el algoritmo LQR, sabiendo que ahora tenemos tres estados, ya que el último corresponde a la integral del error de seguimiento en la velocidad:


```python
# Ecuaciones del sistema con los errores como estados
# Se añade la integral del error en ángulo como tercer estado
Ah = np.block([[A, np.zeros((2, 1))], [-C, 0]])
Bh = np.block([[B], [-D]])

# Controlador LQR para la dinámica del error
Q = np.eye(3)  # Aproximación inicial para 'Q'
Q[0, 0] = 1.0  # Ajustamos peso del ángulo 'phi'
Q[1, 1] = 0.0  # Ajustamos peso de la derivada del ángulo 'phip'
Q[2, 2] = 25   # Peso de la integral del error en velocidad 'xi'
R = 1          # Peso del ángulo de dirección 'u'

# La función 'LQR' también admite usar sólo las matrices A y B
Kh = lqr(Ah, Bh, Q, R)[0]
Kn = Kh[0, :-1]
Ki = -Kh[0, -1]
```

Para simular el sistema, utilizaremos un modelo aumentado de la siguiente forma, que se obtiene al sustituir en la ecuación del sistema el valor de $u$:

$$
    \begin{bmatrix} \dot{\mathbf x} \\ \dot\xi \end{bmatrix} = 
    \begin{bmatrix} \mathbf A - \mathbf{BK} && \mathbf Bk_I \\ D\mathbf K - \mathbf C && -Dk_I \end{bmatrix}
    \begin{bmatrix} \mathbf{x} \\ \xi\end{bmatrix} + 
    \begin{bmatrix} \mathbf 0 \\ 1 \end{bmatrix}r
$$

Antes de simular, verificamos que este sistema es de tipo 1:


```python
# Sistema en lazo cerrado con las ganancias obtenidas
An = np.block([[A - B*Kn, B*Ki], [D*Kn - C, -D*Ki]])
Bn = [[0], [0], [1]]
Cn = [1, 0, 0]
Dn = 0

clisysint = ss(An, Bn, Cn, Dn)

# Función de transferencia del sistema con realimentación
H = ss2tf(clisysint)

# Calculamos G(s) para sistema con realimentación unitaria
minreal(H/(1 - H))
```

Para comprobar que este método es mucho más robusto que la precompensación, se puede hacer una prueba sencilla. En el modelo con precompensación, si antes de ensamblar el sistema *state-space* se simula un error en los parámetros del modelo multiplicando $\bar N$ por un factor, por ejemplo 0.9, se verá que el sistema pasa a ser tipo 0, incluso con valores del factor muy cercanos a la unidad. En cambio, si se hace lo mismo con $k_I$, se observa que el término de grado 0 del denominador se mantiene cercano a cero para cualquier valor de $k_I$.

A continuación se muestra gráficamente la respuesta, comparándola con la obtenida usando precompensación (en línea de puntos). Igual que antes, es interesante probar diferentes pesos en el LQR, para ver cómo varía el comportamiento del sistema.


```python
# Respuesta con misma entrada que en el modelo con precompensación
T, Y, Xn = forced_response(clisysint, t, r)

pf.plot_response(T, Xn, X)
```

Antes de implementar el controlador, hay que asegurarse de que no estamos saturando el actuador:


```python
pf.plot_steering(T, (Ki*Xn[2, :] - Kn@Xn[:2, :]).T)
```

Vemos que los ángulos de dirección se mantienen dentro de valores mucho más realistas. Como siempre, es interesante ver cómo afectan los pesos del LQR, buscando un equilibrio entre una respuesta rápida (pero más brusca) y un comportamiento suave (pero más lento).

A continuación se muestra una animación del nuevo movimiento:


```python
FuncAnimation(pf.fg1, pf.anim, init_func=pf.init, frames=len(T), fargs=(Xn[[0, 2]], dt, h), interval=dt*1000, blit=True)
```

Como todo es correcto, ya podemos utilizar las ganancias para la implementación física del controlador. Se pueden copiar y pegar directamente en un sketch de Arduino, siendo ahora el ángulo de dirección $u= -\mathbf{Kx} + k_I\xi$. Esto significa que, además de los estados anteriores, habrá que ir calculando la integral del error de inclinación, que no es más que ir acumulando en una variable dicho error $\xi = r - \varphi$, multiplicado por el paso de tiempo.


```python
print('const float K1 =%10.6f;  // Ganancia phi'      % Kn[0, 0])
print('const float K2 =%10.6f;  // Ganancia phip'     % Kn[0, 1])
print('const float Ki =%10.6f;  // Ganancia integral' % Ki)
```

## Funcionamiento a diferentes velocidades

Todo lo que se ha hecho hasta ahora ha implicado considerar la velocidad de avance $v$ como un parámetro más del modelo, como la masa o el momento de inercia. Eso quiere decir que las ganancias del controlador que obtengamos estarán optimizadas para funcionar a esa velocidad. En la práctica, lo más probable es que el controlador funcione correctamente en un rango determinado de velocidades, pero si nos alejamos mucho de $v$, empezará a fallar. Una forma de comprobar la sensibilidad del sistema es calcular las ganancias para diferentes velocidades: cuanto menos varíen las ganancias al cambiar la velocidad, más robusto será el controlador.

Si la variabilidad de ganancias es alta y se quiere un controlador que pueda funcionar a diferentes velocidades, habrá que calcular varios vectores de ganancias $\mathbf K_i$, de modo que cada uno tendrá validez en un rango de velocidades. Incluso se puede estudiar la variación de las ganancias en función de la velocidad, e implementar alguna estrategia de interpolación.

# Bibliografía
[1] Ogata, K. *Modern Control Engineering, 5th Ed.*, Pearson, 2010   
[2] Williams II, R. L., Lawrence, D. A. *Linear State-Space Control Systems*, John Wiley & Sons, 2007  
[3] Hespanha, J. P. *Linear Systems Theory, 2nd Ed.*, Princeton University Press, 2018  
[4] Åström, K. J., Murray, R. M., [*Feedback Systems*](http://www.cds.caltech.edu/~murray/amwiki/index.php), Princeton University Press, 2008  
[5] Messner, B. et al. [*Control Tutorials for Matlab and Simulink*](http://ctms.engin.umich.edu)

# Acerca de este documento
Autor: Urbano Lugrís Armesto  
[Laboratorio de Ingeniería Mecánica](http://lim.ii.udc.es/)  
[Escuela Politécnica Superior](https://eps.udc.es/)  
[Universidad de A Coruña](https://www.udc.es/)  

Este documento pertenece a la asignatura *Proyecto Interdisciplinar II*, de cuarto curso de los Grados en Ingeniería Mecánica e Ingeniería en Tecnologías Industriales, impartidos en la Escuela Politécnica Superior de la Universidad de A Coruña.

13958c02331b6bb77d97593fd30818eb39d08b59
ipynb
Jupyter Notebook
Bike.ipynb
ulugris/bike
903f80a0b9a269236867ee22a4dcc1f0bceda446
MIT
Bike.ipynb
ulugris/bike
903f80a0b9a269236867ee22a4dcc1f0bceda446
MIT
Bike.ipynb
ulugris/bike
903f80a0b9a269236867ee22a4dcc1f0bceda446
MIT
Qwen/Qwen-72B
1. YES
2. YES