Adquiriendo señal…
Adquiriendo señal…
Lección 6 de 12· Control y estimación
0 de 12 lecciones · 0 %
En Sistemas APT (lección 7, «Más allá del PID: LQR y robustez») el LQR aparece en tiempo continuo y para un doble integrador, con su forma cerrada , . En Señales y Sistemas (lección 16, «Espacio de estados y sistemas MIMO») se aprende a escribir un sistema como y en la lección 17 («Procesos estocásticos y filtrado de ruido») a razonar sobre ruido. Esta lección junta las tres piezas tal como las implementa la Estación: un LQR discreto a 20 Hz, con acción integral y pre-alimentación, y un filtro de Kalman que le da posición, velocidad y par de perturbación a partir del encoder cuantizado.
Las fuentes son src/atp_ros/atp_ros/control.py (Riccati, LQR integral, Kalman; sin scipy), lqr_controller.py (el nodo) y docs/CONTROL.md §2–§5. El resultado que motiva todo: en el pase N2, el PID da 0.0167° rms y el LQR 0.0070°; con Kalman, 0.0020° (docs/CONTROL.md §5a, escalón C1 de la escalera).
Con , la planta se escribe
El lazo corre a s y mantiene el mando constante entre muestras (retención de orden cero). Con constante en el periodo, la discretización exacta es y . Para este hay forma cerrada; llamando :
Ejemplo numérico. Con , , : y . Entonces
Fíjate en que y : la fricción viscosa resta un poquito en cada paso. control.discretizar_zoh no usa la forma cerrada sino la exponencial de la matriz por bloques , que vale para cualquier ; ambas dan lo mismo.
Un LQR puro sobre es de tipo 1 frente a perturbaciones constantes: un par de fricción o de gravedad deja error permanente. La solución es la misma que en el PID, pero dentro del diseño: se añade el estado , la integral del error de posición, con . El estado aumentado es y
donde es la pre-alimentación (dinámica inversa): el par que la referencia exige por sí sola. La ley completa, tal como la escribe LazoLQR.calcular, es
con y obtenidas de las muestras de la referencia por diferencias hacia atrás de segundo orden (DiferenciadorRef, error ) y el par de sesgo que estima el Kalman.
Queremos el mando que minimiza el costo cuadrático sujeto a (escribimos , por , ). La idea de Bellman: resolver primero un horizonte de un paso, luego dos, y así hacia atrás.
Hipótesis de inducción. El costo óptimo que falta cuando quedan pasos, partiendo de , es una forma cuadrática con simétrica semidefinida positiva. Para no queda nada que optimizar salvo el costo de llegada; tomamos (o ).
Paso de Bellman. Con pasos, el primer mando paga su costo de etapa y deja el resto al óptimo de pasos:
Minimizar. Lo de dentro es una cuadrática convexa en (porque ). Igualamos el gradiente a cero:
Sustituir. Llevando a la expresión y agrupando (usamos que , de modo que los términos cruzados se combinan en uno solo):
que vuelve a ser cuadrática: la hipótesis se cumple para con
Horizonte infinito. Si es estabilizable y detectable, la sucesión converge a un punto fijo : la solución de la ecuación algebraica de Riccati discreta (DARE), y la ganancia estacionaria es
Esta iteración converge linealmente, al ritmo del polo más lento del lazo cerrado al cuadrado; por eso control.dare no la usa tal cual sino el algoritmo de duplicación estructurada (SDA, Chu et al. 2004), que duplica el horizonte en cada iteración y converge cuadráticamente. El punto fijo es el mismo.
# src/atp_ros/atp_ros/control.py (recortado)
LQR_DEFECTO = {'q_theta': 1.0e4, 'q_omega': 1.0, 'q_int': 1.0e3, 'r': 1.0}
def lqr_integral(J=model.J_EJE, b=model.B_EJE, Ts=0.05, ...):
A, B = matrices_eje(J, b)
Ad, Bd = discretizar_zoh(A, B, Ts)
Aa, Ba = aumentar_integral(Ad, Bd, Ts)
Q = np.diag([d['q_theta'], d['q_omega'], d['q_int']])
K, X = lqr_discreto(Aa, Ba, Q, d['r']) # K = (R + B'XB)^-1 B'XA
Acl = Aa - Ba @ K
return {'K': K, 'P': X, 'polos': np.linalg.eigvals(Acl), ...}
Los pesos por defecto siguen la regla de Bryson () ajustada contra el estudio: , . El resultado, que reproducen Python y C++ (docs/ROS2_CONTROL.md §6):
El polo más lento, 0.984, es el del integrador: con s equivale a una constante de tiempo de s, casi cuatro veces más rápida que la integral del PID (12.5 s). La rigidez también sube: N·m/rad frente a . docs/CONTROL.md §2 da el resto del certificado: margen de fase 52.0° en rad/s, margen de retardo 92.5 ms, pico de sensibilidad .
Una advertencia del propio repositorio: con () el margen de fase baja a 44.5° y el LQR sin Kalman oscilaba en la simulación ROS (0.87° rms). Más ganancia no es gratis cuando la velocidad sale de derivar una medida cuantizada.
El LQR necesita , y el encoder solo da cuantizada. Derivar la medida (como el PID, con el filtro de 80 ms) retrasa y amplifica: la cuantización de 0.022° se convierte en saltos de 0.44 °/s en la velocidad. El Kalman de control.FiltroKalman estima por eje con el modelo
donde es el par de sesgo (fricción, gravedad, viento lento) y , ruidos blancos de densidades (rad/s²)²·s y (N·m)²/s, discretizados por Van Loan. Esas densidades son supuestos de ajuste de un barrido, no medidas. La medida es con varianza
la del cuantizador uniforme de la lección 5. Cada paso predice con el mando realmente aplicado (saturado) en el paso anterior y corrige con la medida del mismo sello:
y la covarianza se actualiza en forma de Joseph, , que conserva simetría y semidefinición. La estimación se resta del mando: el LQR cancela la fricción en lugar de esperar a que la integral la descubra. Es el mismo filtro que hereda el MPC de la lección 8.
# src/atp_ros/atp_ros/lqr_controller.py (recortado)
def control(self, s, meas, ref):
if self.est is None:
self.est = control.crear_estimador(self.kalman, self.J, self.b, self.dt,
meas, self.sigma, self.bits, (2,))
estimado = self.est.x
else:
estimado = self.est.paso(meas, self.u) # predice con u_{k-1}
rv, ra = self.dif.paso(ref) # r punto y r dos puntos
self.u = self.lazo.calcular(estimado, ref, rv, ra)
# siempre se responde, con el sello del estado al que responde
El nodo hereda de PidController el emparejamiento por sello: solo cambia la ley. Se elige desde el launch:
ros2 launch atp_ros atp_loop.launch.py el_max:=85 mask:=70 time_scale:=0 controller:=lqr
ros2 launch atp_ros atp_loop.launch.py el_max:=85 mask:=70 time_scale:=0 \
controller:=lqr estimator:=kalman
¿Por qué el LQR + Kalman supera al PID con la planta realista (fricción, holgura y encoder de 14 bits)?
Todos los controladores comparten planta, semilla y pase (docs/CONTROL.md §5, error en el cielo frente al objetivo de 0.1°):
| Caso | PID rms | LQR rms | LQR + Kalman rms |
|---|---|---|---|
| (a) N2 cenital, planta ideal | 0.0167° | 0.0070° | 0.0020° |
| (b) N4b, Ka con ráfaga de 3 N·m | 0.466° | 0.140° | 0.073° |
| (c) planta realista, error real | 0.1045° | 0.0197° | 0.0073° |
Y la lectura honesta que acompaña a la tabla: en la campaña Monte Carlo nominal (d1, 300 pases) el PID es el mejor en p95 (0.0423° contra 0.0460–0.0486°), porque ese presupuesto lo dominan el sesgo del encoder, la latencia y el ruido, que ningún controlador corrige, y el LQR, de mayor ancho de banda, deja pasar más ruido. La ventaja del LQR + Kalman aparece cuando la planta se ensucia.
Este programa hace exactamente la deducción: discretiza el eje con la forma cerrada, arma el estado aumentado e itera la Riccati por programación dinámica hasta el punto fijo. Luego calcula los polos de lazo cerrado como raíces del polinomio característico de . Debe dar , y un polo de módulo 0.9843.
La iteración llega al punto fijo en unas 570 pasadas: es la convergencia lenta que motiva la duplicación de control.dare. Los polos salen en () y , el del integrador, con 3.2 s de constante de tiempo.
El laboratorio LQR y Kalman arranca en «Pase N2 cenital (C1)» con los tres controladores superpuestos sobre el mismo pase. Verifica primero las lecturas: y con tres componentes cerca de . Las barras de rms dan unos 0.0168° (PID) y 0.0067° (LQR); el LQR + Kalman marca 0.0065° sobre el pase completo pero 0.0007° si se descarta el transitorio de arranque de los 3 primeros segundos: el 0.0020° de la Estación queda entre ambos porque allí ese transitorio no se separó. Sube el peso de posición q₁ dos décadas (de a ): la escena de polos muestra los dos polos rápidos acercándose al origen y separándose del eje real, crece y la barra de rms del LQR sin Kalman empeora cuando el encoder es grueso. Elige «Encoder ruidoso (10 bits)» y apaga y enciende el chip LQR + Kalman: la diferencia de rms es el valor de no derivar la medida. Con «Viento Dryden» observa cómo el Kalman convierte la perturbación lenta en y el error vuelve a centrarse.
docs/CONTROL.md §2 son los del lazo con estado real. Diseños robustos (, LQG/LTR) quedan fuera.Hasta aquí el lazo vive en nodos de Python que se hablan por tópicos. En la lección 7 lo movemos a la vía estándar de la robótica: el PID y el LQR como un controlador de ros2_control en C++, y la montura como un plugin de hardware que se cambia con una línea del URDF.
Cierre de la lección
Sigue el recorrido de la lección paso a paso.
Con una cuenta desbloqueas el cuestionario, el foro de dudas y el tutor con IA de esta lección, además del registro de progreso y el certificado verificable al terminar «ROS 2 aplicado: la Estación ATP de punta a punta».