Adquiriendo señal…
Adquiriendo señal…
Lección 5 de 12· Control y estimación
0 de 12 lecciones · 0 %
En Sistemas APT (lección 5, «La montura como robot», y lección 6, «Control PID aplicado al seguimiento») la montura es una ecuación y el PID una ley con tres perillas. En la Estación ATP esa misma ecuación vive en un archivo, src/atp_ros/atp_ros/planta.py, la integra un nodo, mount_sim, y la ley la ejecuta otro nodo, pid_controller, que habla con el primero por tópicos sellados en lockstep (lección 3 de este curso). Si vienes de Señales y Sistemas (lección 11, «Sistemas de segundo orden: control de motor DC»), reconocerás la planta: es un motor con inercia y fricción viscosa.
Esta lección responde tres preguntas de ingeniero, no de libro: qué física exacta simula la Estación y qué parte de ella es un supuesto; cómo está escrito el PID que mueve la montura (con su derivada filtrada y su anti-windup), y por qué ese PID necesita la acción integral para seguir un pase sin quedarse atrás. La fuente de todas las cifras es docs/CONTROL.md §1 y §5, más las ganancias que el firmware copia en firmware/esp32_eje/README.md.
Cada eje (acimut y elevación) se modela igual, en el eje de carga, con la inercia reflejada a través del reductor:
con kg·m², N·m·s/rad y N·m (docs/CONTROL.md §1). El mando es un par y agrupa todo lo que no controlamos: viento, gravedad, fricción no lineal. Dos números salen sin hacer nada:
docs/ROS2_CONTROL.md §8.4: s con la planta simulada y s en Gazebo.La integración es semi-implícita (primero la velocidad, luego la posición con la velocidad nueva) con un subpaso de 2 ms. Este es el camino ideal de PlantaMontura.paso, recortado:
# src/atp_ros/atp_ros/planta.py (recortado)
def paso(self, u, par_ext, h):
"""Avanza h s con el mando u (N m) y el par externo par_ext."""
if not self.avanzada:
# camino original, operacion por operacion (resultados identicos)
alpha = (u + par_ext - self.b * self.omega) / self.J
self.omega = np.clip(self.omega + alpha * h, -self.w_max, self.w_max)
self.theta += self.omega * h
return
ext = par_ext + self._gravedad()
... # friccion de Karnopp, backlash de dos cuerpos
Con los parámetros por defecto todas las capas están apagadas y la planta es idéntica bit a bit a la integración original (test_planta_por_defecto_identica_bit_a_bit). Se encienden con argumentos del launch:
| Capa | Parámetros de mount_sim | Modelo | Estado |
|---|---|---|---|
| Fricción | coulomb_nm, stribeck_nm | Coulomb + Stribeck con adherencia de Karnopp | forma de la literatura; amplitudes supuestas |
| Holgura | backlash_deg | dos cuerpos (motor y carga) con holgura δ | reparto 50/50 de supuesto |
| Desbalance | unbalance_nm | par solo en elevación | supuesto |
| Encoder | encoder_bits | cuantización mid-tread de | exacto |
| Viento | wind_sigma_nm, wind_model | Dryden (Gauss-Markov) o Davenport | forma de la fuente; RMS supuesto |
La honestidad de la tabla importa: el par real de viento sobre un plato de 3 m es de decenas de N·m (78 N·m a 10 m/s según atp.py), muy por encima de lo que aguanta una inercia didáctica de 2 kg·m². Por eso la amplitud del viento es un parámetro y no una deducción. La planta «realista» de los estudios de docs/CONTROL.md §5c usa holgura 0.05°, Coulomb 0.05 N·m + Stribeck 0.02 N·m y encoder de 14 bits.
Un encoder absoluto de bits divide la vuelta en cuentas. El escalón de cuantización es
Por qué su ruido rms es . Si la señal cruza varios escalones entre muestras, el error de redondeo se comporta como una variable uniforme en , de media cero. Su varianza es
así que . Es la misma que usará el Kalman de la lección 6. La lectura práctica de docs/CONTROL.md §8: con 14 bits el escalón ya consume un quinto del presupuesto de 0.1°, y la derivada de la medida lo amplifica por s⁻¹.
mount_sim: integrar exactamente lo que dice el relojEl nodo de la montura no tiene temporizador propio. Integra exactamente el tiempo que avanzó /clock, con el mando vigente retenido (retención de orden cero, como un DAC), publica su estado con ese sello y confirma el mando recibido en /atp/step_ack. Recortado de mount_sim.py:
# src/atp_ros/atp_ros/mount_sim.py (recortado)
def on_clock(self, msg):
now = Time.from_msg(msg.clock)
elapsed = (now - self.t_prev).nanoseconds * 1e-9
if elapsed <= 0.0:
return # /clock repetido o viejo: nada que integrar
self.t_prev = now
n = int(round(elapsed / self.h, 9))
for h in [self.h] * n:
self.planta.paso(self.u, par_ext, h)
self.publish(msg.clock) # /joint_states sellado con t_k
def on_torque(self, msg):
self.u = np.clip(np.array(msg.effort[:2]), -self.u_max, self.u_max)
# se confirma el instante PARA EL QUE se calculo el mando
self.ack_pub.publish(Clock(clock=msg.header.stamp))
Dos detalles de diseño: la saturación a ±8 N·m está en la planta, no solo en el controlador (un mando absurdo no puede mover la montura más de lo físicamente posible), y la posición publicada pasa por la cuantización del encoder, mientras que la verdad solo sale por /atp/mount_truth si se pide truth:=true. Ningún controlador debe leer esa verdad.
pid_controller.pyA s (20 Hz), con ganancias , , , ms y N·m —las mismas que porta el firmware del ESP32—, la ley en cada paso es:
El filtro de primer orden sobre la derivada es imprescindible: derivar la medida cuantizada multiplica el escalón de 0.02197° por 20 en cada muestra. El código real, sin el camino opcional del Kalman:
# src/atp_ros/atp_ros/pid_controller.py (recortado)
e = ref - meas
d_bruta = (e - self.e_prev) / self.dt
alfa = self.dt / (self.tau + self.dt)
self.d_filt += alfa * (d_bruta - self.d_filt)
u_raw = self.kp * e + self.ki * self.integral + self.kd * self.d_filt
self.u = np.clip(u_raw, -self.u_max, self.u_max)
sat = np.abs(u_raw) > self.u_max + 1e-12
# clamping: no integra si el mando esta saturado y el error empuja mas
frozen = self.aw & sat & (u_raw * e > 0.0)
self.integral += np.where(frozen, 0.0, e * self.dt)
self.e_prev = e
Fíjate en lo que el nodo hace después: siempre publica un /atp/torque sellado con el instante del estado al que responde, aunque no le toque calcular. La montura solo confirma el paso si el mando es para su instante; así nunca se empareja la medida de con la referencia de , el sesgo de la lección 3.
Durante un pase la referencia nunca está quieta: en primera aproximación es una rampa . ¿Cuánto se queda atrás el lazo? Trabajamos en Laplace con la planta lineal sin saturar y sin perturbación (a 20 Hz y con el ancho de banda del lazo de ~1 Hz, la aproximación continua conserva el resultado de régimen: el tipo del lazo no cambia al discretizar).
Paso 1: el lazo abierto. La planta es ; ya trae un integrador (la suelta del denominador). Con un controlador PD, , el lazo abierto es
Paso 2: el error. Con realimentación unitaria, , y la rampa es . Si el lazo es estable, el teorema del valor final da
Paso 3: tipo 1. Para el PD, , de modo que
La lectura física es inmediata: para girar a hace falta el par contra la fricción viscosa, y el único término que lo puede dar en régimen es ; luego . Con la rampa del laboratorio, °/s: . En el pase N2 (9 °/s en acimut) serían , casi el doble del presupuesto de 0.1°.
Paso 4: tipo 2. Con la integral, , y el lazo abierto gana un segundo integrador:
La integral acumula el error hasta producir ella sola el par . ¿A qué ritmo? El polinomio característico de lazo cerrado es ; su raíz lenta está muy cerca de s⁻¹, es decir, una constante de tiempo de s. Es el mismo «~kp/ki = 12 s» de recuperación que docs/CONTROL.md §5b mide tras una ráfaga.
Hay una segunda salida, que es la que tomará el LQR de la lección 6: pre-alimentar el par que la rampa exige, . Con la pre-alimentación perfecta un lazo de tipo 1 tampoco tiene error de régimen, porque el término proporcional ya no tiene que producir . Y queda lo que ninguno de los dos resuelve: el PID de tipo 2 sigue la rampa, pero no conoce la aceleración de la referencia, que en un pase cenital cambia rápido.
Con el eje sigue una rampa de 4 °/s con y . ¿Qué error de régimen predice la deducción?
La Estación valida su lazo con escalones reproducibles de scripts/validar.sh. N1 es un solo eje sobre una rampa (el arrastre del PID); N2 es el pase cenital con keyhole, el_max:=85 mask:=70, con 9 °/s de demanda en acimut:
# N1: rampa (el laboratorio usa 2 deg/s; el launch trae 5 por defecto)
ros2 launch atp_ros atp_loop.launch.py profile:=ramp ramp_deg_s:=2 time_scale:=0
# N2: pase cenital en lockstep a maxima velocidad
ros2 launch atp_ros atp_loop.launch.py el_max:=85 mask:=70 time_scale:=0
# la misma corrida con la planta realista de docs/CONTROL.md 5c
ros2 launch atp_ros atp_loop.launch.py el_max:=85 mask:=70 time_scale:=0 \
backlash_deg:=0.05 coulomb_nm:=0.05 stribeck_nm:=0.02 encoder_bits:=14 truth:=true
El monitor de N2 imprime n=1018 rms=0.0167 p95=0.0173 max=0.1382: 0.017° rms, 99.3 % del pase dentro de 0.1°. Ese número es la referencia contra la que se compararán el LQR (lección 6), ros2_control (lección 7) y el MPC (lección 8). Con la planta realista el PID se degrada a 0.1045° rms y solo 67.8 % dentro de 0.1° (docs/CONTROL.md §5c): la fricción estática de 0.07 N·m contra deja una zona de adherencia de rad en la que el PID depende de su integral lenta.
Este programa reproduce la planta (integrada a 1 ms con el mando retenido 50 ms) y el PID de pid_controller.py, con la misma derivada filtrada y el mismo clamping. Compara su salida con el texto: el error de régimen del PD debe dar 0.0400°, la integral lo lleva a cero con una constante de tiempo de unos 12 s, y la cuantización de 14 bits da 0.02197° y 0.00634°.
La salida esperada: el PD se queda en 0.0400° para siempre; el PID baja a 0.0124° a los 10 s, a 0.0024° a los 30 s y a cero; con el encoder de 14 bits el error real en régimen queda en 0.0026° rms. En el escalón de 90°, sin anti-windup el sobreimpulso pasa de 9.3° a 14.0°: la integral acumulada durante la saturación hay que descargarla después.
El laboratorio Planta y PID de un eje arranca en el escenario «N1 rampa 2 °/s» con , , y encoder de 14 bits; las lecturas de cuantización deben marcar y . Lleva el deslizador ki a 0 y mira la escena de error: se aplana en el error de régimen de 0.04° que dedujimos; súbelo de nuevo a 2 y verás la cola exponencial lenta. Cambia velocidad de rampa a 4 °/s con y el error se duplica. Después elige «Escalón 10° (anti-windup)», abre los avanzados y apaga anti-windup: en la franja inferior el par se pega al tope de ±8 N·m y el sobreimpulso crece. Por último, el escenario «Planta realista (Coulomb + holgura + encoder 14 bits)» muestra el atasco y suelta de la zona de adherencia; baja bits del encoder a 10 y observa cómo la derivada convierte la cuantización en ruido de par.
docs/CONTROL.md §8).El PID de la Estación es suficiente con planta ideal, pero su rigidez es baja () y su integral lenta. En la lección 6 lo sustituimos por un controlador que se diseña en lugar de sintonizarse: el LQR con acción integral, y le damos los ojos que le faltan con un filtro de Kalman.
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».