Introducción al Control del Estado-Espacio

Nota

Este artículo es de Ingeniería de Controles en FRC por Tyler Veness con permiso.

De PID a Control Basado en Modelo

Al ajustar los controladores PID, nos centramos en jugar con los parámetros del controlador relacionados con el presente, el pasado y el futuro error (términos P, I y D) en lugar de los estados subyacentes del sistema. Si bien este enfoque funciona en muchas situaciones, es una visión incompleta del mundo.

El control basado en modelos se enfoca en desarrollar un modelo preciso de el system (mecanismo) que estamos tratando de controlar. Estos modelos ayudan a informar gains tomadas de controladores basados en respuestas físicas del sistema, en vez de un arbitrario gain proporcional derivado mediante pruebas. Esto nos permite no solo predecir a futuro como reaccionará el sistema, y también probará nuestros controladores sin un robot que esté físicamente y ahorrar tiempo depurando errores simples.

Nota

El control de estado-espacio hace un uso extensivo de algebra lineal. Más en algebra lineal en teoría del control moderna, incluyendo una introducción a algebra lineal y recursos, puede ser encontrada en el Capítulo 4 de Ingeniería de Controles en FRC.

Si ha usado clases feedforward de WPILib para SimpleMotorFeedforward o sus clases hermanas, o usado Caracterización de FRC para tomar gains de PID para usted, ¡ya está familiarizado con el control basado en modelos! El kv y ka gains puede ser utilizado para describir como un motor (o brazo o transmisión) reaccionará al voltaje. Podemos poner estas constantes en notación estándar de estado-espacio usando LinearSystem de WPILib, algo que haremos en un artículo próximo.

Vocabulario

Para el vocabulario de fondo que se utilizará a lo largo de este artículo, consulte el Glosario.

Introducción a Algebra Lineal

Para una introducción corta e intuitiva a los conceptos básicos de Álgebra Lineal, recomendamos los capítulos 1 al 4 de Series 3Blue1Brown Esencia del algebra lineal (Vectores, ¿qué son esos?, Combinaciones lineales, lapso, y vectores base, Transformaciones lineales y matrices, y Multiplicación de matrices como composición).

¿Qué es Estado-Espacio?

Recuerde que el espacio 2D tiene dos ejes: x y y. Nosotros representamos las localidades en éste espacio como un par de números empacados en un vector, y cada coordenada es una medida de que tan lejos moverse por su eje correspondiente. El estado–espacio es un Sistema cartesiano coordinado con un eje por cada estado de variable, y representamos localidades con el mismo modo que el espacio 2D: con una lista de números en el vector. Cada elemento en el vector corresponde a un estado del sistema. Este ejemplo nos muestra dos ejemplos de vectores de estado en el estado-espacio de un modelo de elevador con los estados \([\text{position}, \text{velocity}]\):

Two vectors in state space with their corresponding arrows.

En ésta imagen, los vectores representando estados en estado-espacio son flechas. Desde ahora en éstos vectores serán representados simplemente por un punto en la punta del vector, pero recuerde que el resto del vector seguirá ahi.

Añadiendo al estado, entradas y salidas son representados como vectores. Dado que el mapeo de los estados actuales y las entradas al cambio de estado es un sistema de ecuaciones, es natural escribirlo en forma de matriz. Esta ecuación matricial se puede escribir en notación de estado-espacio.

¿Qué es la Notación de Estado-Espacio?

La notación de estado-espacio es un conjunto de ecuaciones matriciales que describen como el sistema evoluciona en el tiempo. Estas ecuaciones muestran el cambio en el estado \(\dot{\mathbf{x}}\), y el output \(\mathbf{y}\), para combinaciones lineales del estado actual del vector \(\mathbf{x}\) y input vector \(\mathbf{u}\).

El control del estado-espacio puede soportar sistemas de tiempo continuo y discreto. En el caso de tiempo continuo, la tasa de cambio del estado del sistema \(\mathbf{\dot{x}}\) es expresado como una combinación lineal el estado actual \(\mathbf{x}\) y entrada \(\mathbf{u}\).

En contraste, los sistemas de tiempo discreto expresan el estado del sistema en nuestro siguiente paso del tiempo \(\mathbf{x}_{k+1}\) basado en el estado actual \(\mathbf{x}_k\) y la entrada \(\mathbf{u}_k\), donde \(k\) es el paso de tiempo actual y \(k+1\) es el siguiente paso de tiempo.

Tanto en la forma de tiempo continuo como en la discreta, el vector salida \(\mathbf{y}\) se expresa como una combinación lineal del estado y entrada actual. En muchos casos, la salida es un subconjunto de estados del sistema, y no tiene contribución de la entrada actual.

Cuando modelamos el sistema, primero derivamos la representación del tiempo continuo poruqe las ecuaciones de movimiento son naturalmente escritas como la tasa de cambio de un estado del sistema como una combinación lineal del actual estado y las entradas. Convertimos la representación a tiempo discreto en el robot porque actualizamos el sistema en pasos de tiempo discretos ahí en vez de continuamente.

Los siguientes dos conjuntos de ecuaciones son la forma estándar de la notación estado-espacio del tiempo continuo y tiempo discreto:

\[\begin{split}\text{Continuous: } \dot{\mathbf{x}} &= \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} \\ \mathbf{y} &= \mathbf{C}\mathbf{x} + \mathbf{D}\mathbf{u} \\ \nonumber \\ \text{Discrete: } \mathbf{x}_{k+1} &= \mathbf{A}\mathbf{x}_k + \mathbf{B}\mathbf{u}_k \\ \mathbf{y}_k &= \mathbf{C}\mathbf{x}_k + \mathbf{D}\mathbf{u}_k\end{split}\]
\[\begin{split}\begin{array}{llll} \mathbf{A} & \text{system matrix} & \mathbf{x} & \text{state vector} \\ \mathbf{B} & \text{input matrix} & \mathbf{u} & \text{input vector} \\ \mathbf{C} & \text{output matrix} & \mathbf{y} & \text{output vector} \\ \mathbf{D} & \text{feedthrough matrix} & & \\ \end{array}\end{split}\]

Un sistema de estado-espacio con tiempo continuo puede ser convertido en un sistema de tiempo discreto mediante un proceso llamado discretización.

Nota

En la forma de tiempo discreto, el estado del sistema es mantenido constantemente mediante actualizaciones. Esto significa que solo podemos reaccionar a los disturbios tan rápido como nuestra estimación de estado sea actualizada. Actualizando nuestra estimación más rápido puede ayudar a mejorar la actuación, hasta un punto. La clase Notifier de WPILib se puede utilizar si se desean actualizaciones más rápidas que el bucle principal del robot.

Nota

Mientras que las matrices A, B, C y D de tiempo continuo y discreto de un sistema tengan los mismos nombres, no son equivalentes. Las matrices de tiempo continuo describen la tasa de cambio del estado, \(\mathbf{x}\), mientras que las matrices de tiempo discreto describen el estado del sistema en el siguiente paso de tiempo como una función del estado actual y la entrada.

Importante

El Sistema lineal de WPILib toma las matrices del sistema de tiempo continuo, y las convierte internamente al tiempo discreto donde sea necesario.

Ejemplo de notación de estado-espacio: Volante desde kV y kA

Recuerde que podemos modelar el movimiento de un volante conectado a un motor de corriente continua con la ecuación \(V = kV \cdot v + kA \cdot a\), donde V es la salida del voltaje, v es la velocidad angular del volate y a es la aceleración angular. Esta ecuación puede reescribirse como:math:a = frac{V - kV cdot v}{kA}, o \(a = \frac{-kV}{kA} \cdot v + \frac{1}{kA} \cdot V\). ¿Ve algo familiar? Esta ecuación relata la aceleración angular del volante a su velocidad angular y el voltaje aplicado.

Podemos convertir esta ecuación a notación estado-espacio. Podemos crear un sistema con un estado (velocidad), una entrada (voltaje), y una salida (velocidad). Recuerde que la primera derivada de la velocidad es la aceleración, podemos escribir nuestra ecuación como sigue, reemplazando velocidad con \(\mathbf{x}\), aceleración con \(\mathbf{\dot{x}}\), y voltaje \(\mathbf{V}\) con \(\mathbf{u}\):

\[\mathbf{\dot{x}} = \begin{bmatrix}\frac{-kV}{kA}\end{bmatrix} \mathbf{x} + \begin{bmatrix}\frac{1}{kA}\end{bmatrix} \mathbf{u}\]

¡Eso es todo! Eso es el modelo estado-espacio de un sistema para los que tenemos las constantes kV y kA. Esta misma matemática se usa en la Caracterixación de FRC para modelar volantes y sistemas de velocidad para chasises.

Visualizando las Respuestas del Estado-Espacio: Retrato de Fase

Un retrato de fase puede ayudar a dar intuición visual para la respuesta de un sistema en estado-espacio. Estos vectores en el gráfico tienen sus raíces en algún punto \(\mathbf{x}\) en el estado-espacio, y apunta en la dirección de \(\mathbf{\dot{x}}\), la dirección que el sistema evolucionará pasando el tiempo. Este ejemplo muestra un modelo de un péndulo con los estados del ángulo y velocidad angular.

Para trazar una trayectoria potencial que un sistema puede tomar por el estado-espacio, escoja un punto para empezar y siga las flechas alrededor. En éste ejemplo, empezaremos en \([-2, 0]\). Desde aquí, la velocidad aumenta como se balancea verticalmente y empieza a reducir hasta que alcanza el extremo opuesto del columpio. Este ciclo de giro sobre el origen se repite indefinidamente.

Pendulum Phase Plot with arrows all around going roughly in a circle.

Note que cerca de los bordes del retrato de fase, el eje X se envuelve como una rotación de \(\pi\) radianes en sentido de las manecillas del reloj y una rotación de \(\pi\) radianes en sentido de las manecillas del reloj terminará con el mismo punto.

Para más en ecuaciones diferenciales y retratos de fase, vea el video de 3Blue1Brown Ecuaciones Diferenciales – hacen un gran trabajo de animación de la fase del péndulo alrededor del 15:30.

Visualizando Feedfoward

Este retrato de fase muestra las respuestas de «bucle abierto» del sistema, es decir, cómo reaccionará si dejamos que el estado evolucione naturalmente. Si queremos, digamos, equilibrar el péndulo horizontal (en \((\frac{\pi}{2}, 0)\) en el espacio de estados), necesitaríamos aplicar de alguna manera un control input para contrarrestar la tendencia de bucle abierto del péndulo a oscilar hacia abajo. Esto es lo que el feedforward está tratando de hacer: hacerlo de manera que nuestro retrato de fase tenga un equilibrio en la posición de reference (o punto de ajuste) en el espacio de estado.

Observando nuestro retrato de fase de antes, podemos ver que en \((\frac{\pi}{2}, 0)\) en el estado espacio, la gravedad está empujando el péndulo abajo con torque T, y produciendo aceleración angular hacia abajo con magnitud \(\frac{\tau}{I}\), donde I es el momento de inercia angular del péndulo. Si queremos crear un equilibrio en nuestra referencia de \((\frac{\pi}{2}, 0)\), necesitaremos aplicar una salida que pueda contrarrestar la tendencia natural el sistema a balancear hacia abajo. La meta aquí es resolver la ecuación \(\mathbf{0 = Ax + Bu}\) para \(\mathbf{u}\). Abajo se muestra un retrato de fase donde aplicamos una constante salida que se opone a la fuerza de gravedad en \((\frac{\pi}{2}, 0)\):

Pendulum phase plot with equilibrium at (pi/2, 0).

Control de Retroalimentación

En el caso de los motores de control directo, con solo un modelo matemático y conocimiento de todos los estados actuales del sistema (ej. velocidad angular), podemos predecir todos los futuros estados dados en las entradas de futuros voltajes. Pero si el sistema es cambiado en alguna manera que no esté modelada por nuestras ecuaciones, como una carga o fricción inesperada, la velocidad angular del motor va a derivar del modelo por el tiempo. Para combatir esto, podemos dar al motor comandos correctivos usando el controlador de retroalimentación.

Un controlador de PID es una forma de control de retroalimentación. El control del estado-espacio usa la siguiente ley de control, donde \(\mathbf{K}\) es algún controlador de matriz de ganancia, \(\mathbf{r}\) es un estado de referencia, y \(\mathbf{x}\) es el estado actual en estado-espacio. La diferencia entre estos dos vectores, \(\mathbf{r-x}\), es el error.

\[\mathbf{u} = \mathbf{K(r - x)}\]

Esta ley de control es un controlador proporcional para cada estado del sistema. Controladores proporcionales crean resortes definidos por el software que empujan nuestro estado del sistema hacia nuestro estado de referencia en el estado-espacio. En el caso de que el sistema siendo controlado tenga estados de posición y velocidad, la ley de control de arriba como un controlador PD, que también intenta llevar al error de posición y velocidad a cero.

Veamos un ejemplo de esta ley de control en acción. Usaremos un sistema de péndulo desde abajo. donde el péndulo balanceante rodea al origen en estado-espacio. El caso donde \(\mathbf{K}\) es la matriz cero (una matriz con todos cero) escogería las ganancias P y D de cero – no se aplicaría entrada de control, y el retrato de fase va a verse idéntico al de arriba.

Para añadir algo de retroalimentación, arbitrariamente escogemos \(\mathbf{K}\) of [2, 2], donde nuestra entrada al péndulo es la aceleración angular. Esta K significará que por cada radian de posición error, la aceleración angular será 2 radianes por segundo cuadrado; similarmente, aceleramos por 2 radianes por segundo cuadrado por cada radian por segundo de error. Intente seguir una flecha desde algún punto en el estado-espacio hacia adentro – no importa las condiciones iniciales, el estado sentará en la referencia en vez de rodear sin fin con feedforward pura.

Closed loop pendulum phase plot with reference at (pi/2, 0).

¿Pero cómo podemos elegir una matriz K optima de ganancia para nuestro sistema? Mientras podemos manualmente elegir las ganancias y simular la respuesta del sistema o sintonizarlo en el robot como controlador PID, la teoría de control moderna tiene mejor respuesta: el Regulador Lineal Cuadrático (LQR, en inglés)

El Regulador Lineal Cuadrático

Ya que el control basado en modelo significa que podemos predecir los futuros estados del sistema dada la condición inicial y las entradas de control futuras, podemos tomar una matriz matemática óptima de ganancia \(\mathbf{K}\). Para hacer esto, primero tenemos que definir que tan «bien» o «mal» \(\mathbf{K}\) se vería. Hacemos esto sumando el cuadrado del error y entrada de control con el paso del tiempo, que nos da un número representado que tan «mal» será nuestra ley de control. Si minimizamos la suma, llegaremos a la ley de control óptima.

LQR: Definición

Los REguladores Lineales Cuadráticos funcionan encontrando la ley de control que minimiza el siguiente función de costo , que pesa la suma del error y esfuerzo de control con el paso del tiempo, sujeta a la dinámica lineal del sistema \(\mathbf{\dot{x} = Ax + Bu}\).

\[J = \int\limits_0^\infty \left(\mathbf{x}^T\mathbf{Q}\mathbf{x} + \mathbf{u}^T\mathbf{R}\mathbf{u}\right) dt\]

La ley de control que minimiza \(\mathbf{J}\) puede ser escrita como \(\mathbf{u = K(r - x)}\), donde \(r-x\) es el error.

Nota

Las matrices de diseño \(\mathbf{Q}\) y \(\mathbf{R}\) de LQR no necesitan discretización, pero \(\mathbf{K}\) calcula por tiempo continuo y discreto systems será diferente.

LQR: Afinación

Como los controladores PID pueden ser afinados ajustando sus ganancias, también queremos cambiar como la ley de control balancea el error y la entrada. Por ejemplo, la nave puede querer minimizar el combustible que expande para alcanzar una referencia dada, mientras que un brazo robótico a alta velocidad puede necesitar reaccionar rápidamente a las alteraciones.

Podemos pesar el esfuerzo del error y control de nuestro LQR con las matrices \(\mathbf{Q}\) y \(\mathbf{R}\). En nuestra función de costo (que describe que tan «mal» nuestra ley de control actuará), \(\mathbf{Q}\) y \(\mathbf{R}\) pesa nuestro error y entrada de control relativo a cada uno. En nuestro ejemplo del cohete de arriba, podemos usar \(\mathbf{Q}\) con números relativamente pequeños para mostrar que no queremos penalizar altamente el error, mientras

Con WPILib, las clases de LQR toman el vector de las excursiones de estado máximo deseado y esfuerzos de control y los convierte internamente a unas matrices completas Q y R con la regla de Bryson. Usualmente usamos el caso menor de \(\mathbf{q}\) y \(\mathbf{r}\) para referirse a esos vectores, y \(\mathbf{Q}\) and \(\mathbf{R}\) para referirse a las matrices.

Incrementando los elementos \(\mathbf{q}\) van a hacer los errores largos de LQR menos pesados, y el resultado de la ley de control se comportará mas conservativamente. Esto tiene efecto similar a penalizar el esfuerzo de control más pesado decrementando los elementos \(\mathbf{q}\).

Similarmente, decrementando los elementos \(\mathbf{q}\) van a hacer los LQR penalizar los errores largos más fuerte, y el resultado la ley de control se comportará mas agresivamente. Esto tiene efecto similar a penalizar el esfuerzo de control menos fuerte incrementando los elementos \(\mathbf{q}\).

Por ejemplo, podríamos usar el siguiente Q y R para un sistema de elevador con estados de posición y velocidad.

// Example system -- must be changed to match your robot.
LinearSystem<N2, N1, N1> elevatorSystem = LinearSystemId.identifyPositionSystem(5, 0.5);
LinearQuadraticRegulator<N2, N1, N1> controller = new LinearQuadraticRegulator(elevatorSystem,
    // q's elements
    VecBuilder.fill(0.02, 0.4),
    // r's elements
    VecBuilder.fill(12.0),
    // our dt
    0.020);

LQR: ejemplo de aplicación

Apliquemos un Regulador Lineal Cuadrático a un ejemplo de la vida real. Digamos que tenemos un sistema de velocidad del volante mediante un sistema de identificación para tener \(kV = 1 \frac{\text{volts}}{\text{radianes por segundo}}\) y \(kA = 1.5 \frac{\text{volts}}{\text{radianes por segundo cuadrado}}\). Usando el ejemplo del volante de arriba, tenemos el siguiente sistema lieal:

\[\mathbf{\dot{x}} = \begin{bmatrix}\frac{-kV}{kA}\end{bmatrix} v + \begin{bmatrix}\frac{1}{kA}\end{bmatrix} V\]

Arbitrariamente escogemos la excursión del estado deseado (error máximo) de \(q = [0.1\ \text{rad/seg}]\), y un \(\mathbf{r}\) de \([12\ \text{volts}]\). Después de la discretización con paso de 20ms, encontramos una ganancia de \(\mathbf{K} = ~81\). Esta ganancia K actúa como el componente proporcional del bucle PID en la velocidad del volante.

Ajustemos \(\mathbf{q}\) y \(\mathbf{r}\). Sabemos que incrementando los elementos q o decrementando los elementos \(\mathbf{r}\) que usamos para crear \(\mathbf{Q}\) y \(\mathbf{R}\) hará penalizar nuestro controlador más fuertemente el esfuerzo de control, análogo para intentar manejar un automóvil más conservativamente para mejorar ahorro de combustible. De hecho, si incrementamos nuestro error de tolerancia q desde 0.1 a 1.0, nuestra matriz de ganancia \(\mathbf{K}\) cae desde ~81 to ~11. Similarmente, decrementando nuestro voltaje máximo \(r\) a 1.2 desde 12.0 produce el mismo resultado \(\mathbf{K}\).

La siguiente gráfica muestra la velocidad angular del volante y el voltaje aplicado en el tiempo con dos diferentes ganancias. Podemos ver como una ganancia más alta puede hacer que el sistema alcance la referencia más rápido (a t = 0.8 segundos), mientras dejamos nuestro motor saturado a 12V por más tiempo. Es igual a incrementar la ganancia P de un controlador PID por un factor de ~8x.

Flywheel velocity and voltage over time.

LQR y Medición de Compensación de Latencia

La mayoría del tiempo, nuestros sensores tienen un retraso asociado con sus medidas. Por ejemplo el controlador de motor SPARK MAX sobre CAN puede tener 30ms de retraso asociado con las mediciones de velocidad.

Este retraso significa que nuestro controlador de retroalimentación va a estar generando comandos de voltaje basados en estimaciones de estado del pasado. Regularmente tiene el efecto de introducir inestabilidad y oscilaciones en el sistema, como se muestra en la gráfica de abajo.

Como sea, podemos modelar nuestro controlador para controlar donde el estado del sistema está retrasado en el futuro. Esto reducirá la matriz de ganancia LQR \(\mathbf{K}\), intercambiando la actuación del controlador por estabilidad. La fórmula de abajo, que ajusta la matriz de ganancia para contar el retraso, también es usado en la frc-characterization.

\[\mathbf{K_{compensated}} = \mathbf{K} \cdot \left(\mathbf{A} - \mathbf{BK}\right)^{\text{delay} / dt}\]

Multiplicando \(\mathbf{K}\) por \(\mathbf{A} - \mathbf{BK}\) esencialmente avanza las ganancias por un paso de tiempo. En este caso, multiplicamos por \(\left(\mathbf{A} - \mathbf{BK}\right)^{\text{delay} / dt}\) para avanzar las ganancias por el retraso de las mediciones.

Flywheel velocity and voltage with dt=5.0ms and a 10.0ms delay.

Nota

Esto puede tener efecto de reducir \(\mathbf{K}\) a cero, de manera efectiva inhabilitando el control de retroalimentación.

Nota

El controlador de motor SPARK MAX utiliza un filtro FIR de 40 tomas con un retraso de 19,5 ms, y los marcos de estado se envían por defecto cada 20 ms.

El siguiente código muestra cómo ajustar la ganancia K del controlador LQR para los retardos de entrada del sensor:

// Adjust our LQR's controller for 25 ms of sensor input delay. We
// provide the linear system, discretization timestep, and the sensor
// input delay as arguments.
controller.latencyCompensate(elevatorSystem, 0.02, 0.025);

Linealización

La linealización es una herramienta usada para aproximar las funciones no lineales y el sistema de estado-espacio usando las lineales. En el espacio bidimensional, las funciones lineales son líneas derechas mientras las funciones no lineales son curvas. Un ejemplo común de la función no lineal y correspondiente aproximación lineal es \(y=\sin{x}\). Esta función puede estar aproximada por \(y=x\) cercano a cero. Esta aproximación es precisa mientras esté cerca de \(x=0\), pero pierde la precisión mientras más nos alejamos del punto de linealización. Por ejemplo, la aproximación \(\sin{x} \approx x\) es precisa a 0.02 - 0.5 radianes de \(y = 0\), pero rápidamente pierde la precisión después de eso. La siguiente imagen, podemos ver:math:y =sin{x}, \(y=x\) y la diferencia entre la aproximación y el valor real de \(\sin{x}\) en \(x\).

Three plots showing sin(x), x, and sin(x) - x.

Podemos también linealizar el sistema estado-espacio con dinámicas no lineales. Hacemos esto eligiendo un punto \(\mathbf{x}\) en el estado-espacio y usando esto como entrada para nuestras funciones no lineales. Como el ejemplo de arriba, esto funciona bien para los estados cerca del punto sobre el cual se linealiza el sistema, pero podemos divergir rápidamente lejos de ese estado.