Tutorial · Avanzado · 2 horas
Control PID en cascada para un robot autoequilibrado
Los dos lazos PID anidados de un robot equilibrista: un lazo interno fija el ángulo de inclinación y uno externo pide inclinarse para controlar la posición.
Introducción
Ya tienes un ángulo de inclinación fiable. Ahora necesitas mantener el robot de pie sobre él —y luego, con el tiempo, hacer que se quede en un sitio en lugar de derivar por la habitación manteniéndose técnicamente vertical.
Son dos trabajos distintos, y el error que casi todo el mundo comete primero es intentar hacerlos con un solo lazo. Este tutorial los construye como dos, anidados, que es como se controlan los robots equilibristas reales.
Por qué un solo lazo no puede funcionar
Lo que quieres es un robot que se quede donde lo pones. Lo que puedes comandar es el par de los motores. Entre medias hay un hecho incómodo: la única forma de cambiar la posición del robot es cambiar primero su inclinación.
Intenta controlar la posición directamente —maneja los motores en proporción al error de posición— y las ruedas salen disparadas por debajo del centro de masas. El cuerpo gira al revés y el robot cae al instante, habiéndose movido en dirección contraria al caer. Esto no es un problema de sintonía; el signo de la respuesta inicial está genuinamente invertido, una propiedad llamada fase no mínima.
La salida es controlar las dos cantidades a distintas velocidades, en cascada.
La estructura
position error → [ OUTER PID ] → tilt target
↓
tilt error → [ INNER PID ] → motor command → robot
↑
measured tilt
El lazo interno hace un trabajo: mantener el ángulo de inclinación que se le pida. Funciona rápido —200 Hz o más— porque lucha contra la gravedad, y la gravedad no espera.
El lazo externo hace el otro trabajo: llevar el robot a una posición. No puede tocar los motores, así que trabaja pidiendo al lazo interno una pequeña inclinación. Detrás de donde quiere estar, pide una inclinación adelante; el lazo interno maneja las ruedas para mantener esa inclinación; mantener una inclinación adelante significa acelerar continuamente hacia adelante. Funciona quizá diez veces más lento que el lazo interno.
Esa separación de tasas es todo el diseño. Si el lazo externo está cerca de la velocidad del interno, luchan —el lazo externo cambia el objetivo más rápido de lo que el interno puede seguirlo, y el robot oscila hasta el suelo.
El lazo interno
float angleError = angle - tiltTarget; // measured MINUS target
angleIntegral = constrain(angleIntegral + angleError * dt, -3, 3);
float command = Kp * angleError
+ Ki * angleIntegral
+ Kd * gyroRate;
command = constrain(command, -255, 255);Tres detalles ahí importan más que las ganancias.
El error es medida menos objetivo, lo contrario del setpoint − measurement de libro. Para atrapar un robot que cae adelante manejas las ruedas adelante, bajo la masa que cae, igual que mueves la mano bajo una escoba. Así que una inclinación positiva debe producir un comando de motor positivo. Invierte esto y el controlador maneja el robot contra el suelo en cuanto lo enciendes —un fallo distintivo y violento que casi siempre es este signo.
El término derivativo usa directamente la tasa del giróscopo, no una diferencia de ángulos sucesivos. El giróscopo mide velocidad angular como salida nativa. Diferenciar una señal ruidosa de ángulo amplifica justo el ruido que menos quieres amplificado, y es la razón por la que muchos primeros intentos necesitan una ganancia D sospechosamente baja para mantenerse estables.
La integral está limitada. Durante una caída los motores saturan y el error sigue creciendo, así que una integral sin límite se enrolla hasta un número enorme que luego lucha contra la recuperación al volver. Limítala, y deja de acumular por completo mientras la salida esté saturada.
El lazo externo
if (++slowTick >= 10) { // ten times slower
slowTick = 0;
float positionError = targetPosition - wheelPosition;
tiltTarget = constrain(KpPos * positionError - KdPos * wheelVelocity,
-0.12, 0.12); // radians, about ±7°
}wheelPosition viene de sumar tus cuentas de encoder. Si no tienes encoders puedes integrar el comando del motor como aproximación gruesa, pero el lazo será notablemente peor.
El límite no es opcional. Un lazo externo al que se permite pedir 30° entrega al interno un objetivo que físicamente no puede mantener —el robot se compromete con una inclinación de la que no puede recuperarse y cae obedeciendo órdenes perfectamente. Unos pocos grados bastan; una inclinación de 7° acelera un robot con brío.
Fíjate en que el término de velocidad está restado. Amortigua la aproximación para que el robot no pase de largo su objetivo y tenga que volver.
Sintonía, en el orden que funciona
Sintoniza primero el lazo interno solo. Pon KpPos y KdPos a cero para que el lazo externo quede inerte y tiltTarget se mantenga a cero. El robot se equilibrará en el sitio y derivará lentamente —eso es esperado y está bien por ahora.
- Ki y Kd a cero. Sube Kp hasta que el robot reaccione con brío a un empujón. Demasiado bajo y se hunde hasta el suelo sin luchar de verdad; demasiado alto y zumba.
- Sube Kd hasta que la oscilación se amortigüe. Este es el término que convierte un robot que vibra hasta caerse en uno que se asienta. En el simulador de equilibrio puedes poner D a cero en un robot en marcha y contar las oscilaciones antes de que caiga.
- Sube Ki al final, y con suavidad. Elimina la inclinación lenta causada por el trim de montaje de la IMU. Demasiado y el robot desarrolla una lenta oscilación creciente al sobreimpulsar y desenrollarse la integral.
Solo entonces habilita el lazo externo, y empieza KpPos en una décima de lo que parezca razonable. Su efecto es indirecto —observas que el robot deje de derivar, no una respuesta enérgica.
Modos de fallo y qué significan
| Síntoma | Casi siempre |
|---|---|
| Golpea el suelo al instante al encender | Signo del lazo interno invertido |
| Oscila con amplitud creciente hasta caer | Kd demasiado baja, o D tomada de ángulo diferenciado en lugar del giróscopo |
| Zumba o vibra en vertical | Kp demasiado alta, o tasa de lazo demasiado lenta para las ganancias |
| Se equilibra pero conduce lentamente por la habitación | Trim de montaje de la IMU, con Ki demasiado baja para absorberlo |
| Se equilibra, y de pronto da un tirón al cabo de unos segundos | Enrollamiento de la integral —limítala |
| Se hunde suavemente al suelo sin luchar | Kp demasiado baja, o motores saturando en su límite |
| Bien solo, oscila cuando el lazo externo entra | Lazo externo demasiado rápido o fuerte; enlentécelo y limita más su salida |
Corta los motores cuando ya no hay recuperación
if (fabs(angle) > FALL_ANGLE) { // about 35 degrees
driveMotors(0, 0);
angleIntegral = 0;
return;
}Pasados unos 35° la aceleración horizontal necesaria para volver bajo el centro de masas supera lo que las ruedas pueden dar antes de que el cuerpo llegue al suelo. El controlador no lo sabe y forzará a par máximo todo el camino hacia abajo, lo que es duro para las reductoras y hace el fallo más ruidoso de lo necesario.
Cortar también reinicia la integral, así que recoger el robot te devuelve un lazo limpio en lugar de uno que aún arrastra una corrección enrollada de la caída.
A dónde ir después
Las ganancias aquí interactúan más fuerte que en un PID de un solo lazo, porque la planta es inestable —no hay un «más o menos bien» que simplemente rinda mal. O se mantiene de pie o no.
Eso hace un simulador inusualmente útil: puedes poner D a cero en un robot que se está equilibrando, observar el fallo exacto y deshacerlo sin reparar nada. Prueba los preajustes en el simulador de equilibrio, y luego construye la cosa real en el proyecto de robot autoequilibrado.
Explora el grafo
Forma parte de estos montajes
Proyectos y rutas de aprendizaje que incluyen este tutorial.
Lecturas adicionales