# Modelo matemático del sistema

El péndulo invertido es un ejemplo clásico de control de sistema inestable. En su forma más básica, se puede modelizar el vehículo como un mecanismo con movimiento plano, en el que una barra se conecta a una rueda a través de un motor, como se muestra en la figura:



La rueda tiene radio $r$, masa $m$, y momento de inercia $I$. La barra, cuyo centro de gravedad se encuentra a una distancia $l$ del eje de la rueda, tiene masa $M$ y momento de inercia $J$. Entre la rueda y la barra se introduce un par $\tau_m$ por medio de un motor.

A continuación se desarrollarán las ecuaciones que modelizan la dinámica del sistema, que serán necesarias para poder diseñar el controlador. En este proyecto vamos a emplear un controlador LQR (*Linear Quadratic Regulator*), que requiere que las ecuaciones del sistema estén expresadas de la siguiente forma, denominada representación *state-space*:

$$
\begin{align}
    \dot{\mathbf x} &= \mathbf A\mathbf x + \mathbf B\mathbf u \\
    \mathbf y &= \mathbf C\mathbf x + \mathbf D\mathbf u
\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 unas entradas o *inputs* $\mathbf u$. Las matrices $\mathbf A$ y $\mathbf B$ son constantes, de modo que se trata de un sistema de ecuaciones diferenciales lineales con coeficientes constantes.

La segunda ecuación relaciona las salidas o *outputs* $\mathbf y$, es decir, las magnitudes del sistema que queremos conocer y/o controlar, con el estado del sistema y los *inputs*, a través de dos matrices $\mathbf C$ y $\mathbf D$, también constantes.

Como se verá más adelante, nuestro sistema no es lineal, así que habrá que *linealizarlo*, es decir, aproximar sus ecuaciones a un sistema lineal en el formato estándar, que será válido alrededor de la posición de equilibrio.

## Ecuaciones del movimiento

Las ecuaciones del movimiento del sistema las podemos obtener utilizando el método de Lagrange. Para ello, lo primero es establecer un conjunto mínimo de variables que definan el estado del sistema. Si asumimos rodadura sin deslizamiento, el sistema tiene dos grados de libertad, así que se puede describir su estado a partir de las dos coordenadas indicadas en la figura: la posición del eje, $x$, y el ángulo de la barra respecto a la vertical, $\theta$.

En lugar de desarrollar las ecuaciones de Lagrange a mano, se utilizará SymPy, un CAS (*Computer Algebra System*) para Python. Lo primero es cargar los módulos correspondientes e inicializar las variables del sistema. Las variables dependientes del tiempo se definen utilizando el submódulo de Mecánica, para poder utilizar la notación de Newton (puntos sobre las variables para indicar derivación respecto al tiempo):


```python
from sympy import symbols, Matrix, sin, cos, solve, linsolve, lambdify
from sympy.physics.mechanics import dynamicsymbols, init_vprinting

# Activamos notación de Newton
init_vprinting()

# Definimos las constantes del sistema y el tiempo
r, l, M, m, I, Im, J, L, R, g, Kt, Kv, b, t = symbols('r l M m I Im J L R g K_t K_v b t')

# Definimos variables dinámicas (dependientes del tiempo)
x, th, V, i, tm, wm = dynamicsymbols('x theta V i tau_m omega_m')

# Guardamos derivadas respecto al tiempo en variables auxiliares
thp = th.diff(t)
ths = th.diff(t, 2)
xp = x.diff(t)
xs = x.diff(t, 2)
```

Lo primero que hay que hacer es expresar la energía cinética y potencial del sistema en función de las coordenadas generalizadas $x$ y $\theta$:


```python
# Velocidad del CDG de la barra (derivada de la posición)
vb = Matrix([x + l*sin(th), l*cos(th)]).diff(t)

# Energía cinética de la barra
Tb = (M*vb.dot(vb) + J*thp**2)/2

# Energía cinética de la rueda
Tr = (m*xp**2 + I*(xp/r)**2)/2

# Energía potencial de la barra
Vb = M*g*l*cos(th)
```

Después, se obtienen las fuerzas generalizadas $Q_\theta$ y $Q_x$ correspondientes al par $\tau_m$:


```python
# Fuerzas generalizadas asociadas al par del motor
Qt = -tm  # Efecto sobre el ángulo 'theta'
Qx = tm/r # efecto sobre la posición 'x'
```

Con todos estos términos, ya se puede montar la función Lagrangiana y obtener las ecuaciones del movimiento:


```python
# Función Lagrangiana L = T - V
L = Tb + Tr - Vb

# Ecuaciones del movimiento (vector de ecuaciones iguales a cero)
eqs = Matrix([L.diff(thp).diff(t) - L.diff(th) - Qt, L.diff(xp).diff(t) - L.diff(x) - Qx])
```

Simplificando y reordenando términos, se obtiene el siguiente sistema no lineal de ecuaciones diferenciales de segundo orden en $\theta$ y $x$:

$$
\begin{align}
    \left(J + Ml^2\right)\ddot\theta + Ml\cos\theta\ddot x - Mgl\sin\theta &= -\tau_m \\
    Ml\cos\theta\ddot\theta + \left(m + M + \frac{I}{r^2}\right)\ddot{x} - Ml\sin\theta\dot{\theta}^2 &= \frac{\tau_m}{r}
    \label{eq:motion}
\end{align}
$$

Con este sistema de ecuaciones se podría simular la dinámica del sistema para un par motor $\tau_m$ variable en el tiempo, partiendo de unas condiciones iniciales cualesquiera $x_0$, $\dot x_0$, $\theta_0$, $\dot\theta_0$.

## Introducción del modelo de motor

Cuando el par $\tau_m$ (el *input* del sistema) se produce en un motor DC, no se puede establecer su valor directamente. Normalmente, lo que podemos utilizar en su lugar como variable de control $u$ es el voltaje $V$ aplicado a los bornes, o el voltaje promedio si se usa un amplificador con salidas PWM. Pero el par no depende sólo del voltaje, también varía con la velocidad de giro $\omega_m$. Las ecuaciones que modelizan el comportamiento del motor se explican en detalle en el [documento correspondiente](./Motor.ipynb), y son las siguientes:

$$
\begin{align}
    \tau_m &= K_ti - I_m\dot\omega_m - b\omega_m \\
    V &= Ri + K_v\omega_m
\end{align}
$$

donde $i$ es la corriente, $R$ es la resistencia del bobinado, $K_t$ es la constante de par, $K_v$ es la constante de velocidad, $I_m$ es el momento de inercia equivalente del rotor, y $b$ es el coeficiente de rozamiento viscoso.

Lo primero que vamos a hacer es eliminar la corriente de las ecuaciones, despejándola en la segunda ecuación y sustituyéndola en la primera, utilizando la función `subs` de SymPy:


```python
# Ecuación del par del motor (expresión igual a cero)
eqm = Kt*i - Im*wm.diff(t) - b*wm - tm

# Sustituimos la corriente por su valor despejado en la segunda ecuación
eqm = eqm.subs(i, (V - Kv*wm)/R)
```

La ecuación resultante relaciona el par motor con el voltaje y la velocidad de giro. Esta última, que es la que nos proporcionará el *encoder*, se puede calcular a partir de las variables del sistema de la siguiente manera, teniendo en cuenta que se trata de la velocidad angular relativa entre la barra y la rueda:

$$
    \omega_m=\frac{\dot x}{r} - \dot\theta
$$

Si se sustituyen $\omega_m$ y su derivada en la ecuación del motor, se puede expresar el par motor como función del voltaje y las variables del sistema. Una vez despejado $\tau_m$ utilizando la función `solve`, lo sustituimos en las ecuaciones del sistema:


```python
# Sustituimos 'wm' y su derivada en la ecuación del motor
eqm = eqm.subs(wm, xp/r - thp)

# Despejamos el par y lo sustituimos en las ecuaciones del movimiento
eqs = eqs.subs(tm, solve(eqm, tm)[0]).doit()

# Hay que ejecutar la función 'doit()' para que SymPy evalúe completamente todas las derivadas
# Si no se hace, algunas segundas derivadas tipo 'd2x/dt2' quedarán en la forma 'd/dt(dx/dt)'
# En ese caso, al linealizar fallará al calcular el Jacobiano, porque sólo reconoce 'd2x/dt2' como 'xs'
```

## Linealización del modelo alrededor de la posición de equilibrio

Ahora procedemos a linealizar las ecuaciones alrededor de la posición de equilibrio, utilizando los dos primeros términos de la serie de Taylor. La linealizacion de una función vectorial $\mathbf f\left(\mathbf x\right)$ alrededor de una posición de equilbrio $\mathbf x_0$ se obtiene como:

$$
    \mathbf f\left(\mathbf x\right) \approx \mathbf f\left(\mathbf x_0\right) +
    \mathbf F\left(\mathbf x_0\right)\left(\mathbf x - \mathbf x_0\right)
$$

donde la matriz Jacobiana $\mathbf F\left(\mathbf x\right)$ es tal que:

$$
    \mathbf F_{ij}\left(\mathbf x\right) = \frac{\partial \mathbf f_i\left(\mathbf x\right)}{\partial x_j}
$$

Utilizaremos la función `jacobian` de SymPy para calcular automáticamente la matriz Jacobiana. Luego, sólo hay que evaluar $\mathbf f$ y $\mathbf F$ en la posición de equilibrio (cero en nuestro caso) y construir la serie de Taylor:


```python
# Vector de variables
var = Matrix([th, x, thp, xp, ths, xs])

# Matriz Jacobiana del sistema
jac = eqs.jacobian(var)

# Evaluación de ecuaciones y matriz Jacobiana en posición de equilibrio
e0 = eqs.subs({v: 0 for v in var})
j0 = jac.subs({v: 0 for v in var})

# Sistema linealizado: dos primeros términos de la serie de Taylor
lsys = e0 + j0*var
```

## Representación *state-space*

Ya tenemos las ecuaciones del movimiento linealizadas alrededor de la posición de equilibrio, en forma de sistema de ecuaciones diferenciales lineales con coeficientes constantes. Para poder diseñar el controlador, queremos reescribir este sistema usando la estructura estándar $\dot{\mathbf x}=\mathbf{Ax}+\mathbf{B}u$. A partir de aquí consideraremos el *input* $u$ como un escalar, ya que nuestro sistema sólo tiene una entrada: el voltaje del motor $V$. Las ecuaciones del movimiento son de segundo orden, así que tendremos que establecer el vector de estados de forma que se puedan expresar como un sistema de primer orden. En el sistema linealizado aparecen $\theta$, $\dot\theta$, $\ddot\theta$, $\dot x$ y $\ddot x$, de modo que podemos elegir los estados de la siguiente manera:

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

El primer elemento de $\dot{\mathbf x}$ ya es directamente el segundo de $\mathbf x$. Por lo tanto, sólo nos falta expresar los otros dos elementos $\ddot\theta$ y $\ddot x$ en función de $\mathbf x$ y $u$ . Para eso, hay que resolver un sistema lineal de dos ecuaciones con dos incógnitas, usando la función `linsolve`:
        


```python
thsxs = Matrix(linsolve(lsys, [xs, ths]).args[0][::-1])
```

Una vez conocemos $\ddot\theta$ y $\ddot x$ en función de $\mathbf x$ y $u$, podemos determinar las matrices constantes $\mathbf A$ y $\mathbf B$. Para calcular $\mathbf A$, ponemos la solución para $\ddot\theta$ y $\ddot x$ en forma matricial, y añadimos una fila encima para $\dot\theta$. Como sabemos que la ecuación es lineal, podemos usar `jacobian` para expresarla en forma matricial:


```python
A = Matrix([[0, 1, 0]]).row_insert(1, thsxs.jacobian([th, thp, xp]))
```

Para calcular $\mathbf B$, hacemos lo mismo pero derivando respecto al *input*, y añadimos un cero en la primera fila:


```python
B = Matrix([0]).row_insert(1, thsxs.jacobian([V]))
```

Con esto ya tenemos las expresiones de las matrices $\mathbf A$ y $\mathbf B$, que dependerán únicamente de los parámetros del sistema. El controlador necesita también las ecuaciones de salida, que dan los *outputs* del sistema en la forma $\mathbf y = \mathbf C\mathbf x + \mathbf Du$. En nuestro caso, los *outputs* (variables que queremos controlar) son directamente los estados $\theta$ y $\dot x$, así que las matrices $\mathbf C$ y $\mathbf D$ son muy fáciles de obtener:


```python
# Matriz de salida: ángulo 'th' y velocidad 'xp' (2x3)
Cn = [[1, 0, 0], [0, 0, 1]]

# Matriz de transferencia directa (2x1)
Dn = [[0], [0]]
```

Si queremos diseñar un controlador para un sistema real, ahora hay que darles valores numéricos. En este ejemplo se muestran unos valores orientativos de los parámetros, los reales habrá que identificarlos lo mejor posible con los medios a nuestro alcance (báscula, medición de oscilaciones, hojas de características, etc.):


```python
import numpy as np

par = {m: 4e-2, M: 3e-1, I: 4e-5, Im: 2e-4, J: 4e-4, l: 2e-2, r: 4e-2, g: 9.81}
par.update({Kt: 2.0e-1, Kv: 1.5e-1, R: 4.0, b: 3e-4})

# Convertimos expresiones simbólicas en funciones de los parámetros y las evaluamos
An = lambdify([par.keys()], A)(par.values())
Bn = lambdify([par.keys()], B)(par.values())

# Guardamos matrices numéricas en fichero, para el controlador
np.savez('./Data/Model.npz', A=An, B=Bn, C=Cn, D=Dn, r=par[r])
```

Una vez generadas las matrices del sistema, se puede proceder al [diseño del controlador](./Control.ipynb).

e99c8f49a2e074b6c6dc73a087f6e53e8955439c
ipynb
Jupyter Notebook
Model.ipynb
ulugris/balboa
97839ca001baeb27a33d16c1c0a0c4f22f36b60c
MIT
Model.ipynb
ulugris/balboa
97839ca001baeb27a33d16c1c0a0c4f22f36b60c
MIT
Model.ipynb
ulugris/balboa
97839ca001baeb27a33d16c1c0a0c4f22f36b60c
MIT
Qwen/Qwen-72B
1. YES
2. YES