Robot autoequilibrado con ESP32: tres lazos de control

Este robot acaba de saltar de una báscula de baño y sigue de pie. Cinco días antes ni siquiera podía quedarse en un sitio. En ese tiempo su hardware no cambió en nada: el mismo chasis, los mismos motores, el mismo sensor. Solo cambió el programa. Un lazo de control se convirtió en dos, luego en tres, y cada lazo nuevo resolvía lo que el anterior no sabía hacer.

Es un robot autoequilibrado de dos ruedas: un chasis de tres pisos impreso en 3D sobre dos motorreductores de CC, una placa ESP32 y un sensor de movimiento. Lo construimos para comparar distintas formas de mantener un robot erguido con el mismo hardware, empezando por la clásica, el PID. Este artículo recorre los diez días que van de la primera prueba al salto, en el orden en que ocurrieron: qué lleva dentro, cómo sabe el robot dónde es arriba, los tres lazos uno a uno y lo que nos enseñó conducirlo por Wi-Fi. Las cifras salen de los propios registros del robot.

El robot

El chasis está formado por tres pisos impresos, apilados uno sobre otro, cada uno una pieza aparte. Lo dibujamos en Fusion 360 junto con las piezas compradas, así que los motores y los drivers ya tenían su sitio antes de imprimir nada.

De abajo hacia arriba: los motores cuelgan bajo el piso inferior, en los soportes de su kit. El primer piso lleva los dos drivers de motor, con los disipadores hacia abajo. El segundo piso es el cerebro: la placa ESP32 con su pequeña pantalla sobre una placa perforada, el sensor de movimiento y un buzzer. El piso de arriba lleva una batería de tres celdas con su placa de protección, un interruptor y un módulo reductor de 5 V. El cuerpo mide 164 × 128 × 60 mm, 235 mm de ancho con las ruedas, y el robot entero pesa 1025 g.

Dos detalles de la transmisión marcaron todo lo que vino después. El primero es el retardo. El sensor entrega una muestra nueva cada 2 ms, y la orden que resulta de ella llega a los drivers en menos de un milisegundo, pero una rueda necesita alrededor de una décima de segundo para responder a un cambio de orden. El segundo es la reductora: tiene holgura, así que tras cada cambio de sentido el motor gira un instante antes de que lo haga la rueda.

Y los dos motores no son gemelos. En una grabación a ras de suelo, una misma orden de PWM hizo oscilar la rueda lejana 18 mm y la cercana 8 mm. La orden era idéntica; por qué la respuesta no lo es, no lo hemos determinado. Si no se corrige, la diferencia hace que el robot gire despacio sobre sí mismo mientras se equilibra, y de eso se encarga una pequeña corrección de rumbo, descrita en el apartado sobre la conducción.

Dónde es arriba

Todo lo que hace un robot que se mantiene en equilibrio depende de un número: su inclinación. El MPU-6500 ofrece dos maneras de obtenerla, y ninguna funciona por sí sola.

El giroscopio mide a qué velocidad gira el cuerpo. Suma sus lecturas cada 2 ms y obtienes un ángulo suave y rápido, pero que deriva: sin corregir el desfase propio del giroscopio medimos unos 15° por minuto. El acelerómetro sabe dónde está la gravedad, así que su ángulo no puede derivar, pero también nota cada sacudida. Con los motores en marcha, su ángulo saltaba de un lado a otro en un rango de 18,7°.

Un filtro complementario toma la mitad buena de cada uno. En cada paso, el giroscopio hace avanzar el ángulo, y luego el ángulo se acerca a la lectura del acelerómetro en una parte diminuta de la diferencia, una milésima. Dicho de otro modo: 99,9 % giroscopio, 0,1 % acelerómetro, lo que equivale a una constante de tiempo de dos segundos. En el banco de pruebas, el ángulo filtrado se mantuvo dentro de un margen de seis centésimas de grado.

En el firmware todo se reduce a unas pocas líneas. La placa del sensor va montada de canto y ladeada 1,4°, así que antes que nada el código separa la velocidad de cabeceo, que necesita el filtro, de la velocidad de guiñada, que más adelante usa el código de giro:

// Leaning forward, the forward axis dips below the horizontal:
// its reading is -sin(theta).
att.thetaAcc = atan2f(-(float)s.accel[1], (float)s.accel[0])
               * DEG_PER_RAD;

// The board is rolled by 1.4 deg: take pitch and yaw rates apart.
const float gx = s.gyro[0] / GYRO_LSB_PER_DPS - att.gyroBiasDps[0];
const float gz = s.gyro[2] / GYRO_LSB_PER_DPS - att.gyroBiasDps[2];
att.omegaPitch = gz * rollCos - gx * rollSin;
att.omegaYaw = gx * rollCos + gz * rollSin;

// Complementary filter: the gyroscope carries the angle from
// sample to sample, the accelerometer pulls it back to gravity.
// FILTER_K = dt / (tau + dt) = 0.002 / 2.002, about 0.1 %.
att.theta = wrapDeg(att.theta + att.omegaPitch * IMU_DT);
att.theta = wrapDeg(att.theta
                    + FILTER_K * wrapDeg(att.thetaAcc - att.theta));

El paso va al ritmo del propio reloj del sensor. Un temporizador por hardware despierta la tarea de control justo antes de cada muestra esperada, la tarea la lee y ejecuta todo el controlador, y la pantalla y el registro del ensayo esperan su turno con menor prioridad. El Wi-Fi vive en el otro núcleo. El esquema viene de nuestro anterior balancín de hélice, construido con la misma placa y el mismo sensor.

Primer lazo: el ángulo

El primer experimento usa un solo lazo que vigila únicamente el ángulo. Es un controlador PID que se ejecuta 500 veces por segundo, una vez por cada muestra del sensor, y su salida es la potencia para los dos motores, en porcentaje:

• P, el término proporcional: kp = 30, así que un grado de error pide el 30 % de la potencia máxima.
• I, el término integral: ki = 30 acumula el error a lo largo del tiempo y sigue empujando mientras este dure.
• D, el término derivativo: kd = 0,3 calma el balanceo. Toma la velocidad directamente del giroscopio en lugar de derivar un ángulo ruidoso.

La consigna no es exactamente cero. El centro de masas no queda perfectamente sobre el eje, así que el robot se equilibra en θ_ref = 0,3°.

float pidStep(float theta, float omegaPitch, float thetaRef,
              const Params &p, float dt, float *raw) {
    const float error = theta - thetaRef;
    const float summed = integral + p.ki * error * dt;
    integral = constrain(summed, -p.uMax, p.uMax);
    const float u = p.kp * error + integral + p.kd * omegaPitch;
    *raw = u;
    return constrain(u, -p.uMax, p.uMax);
}

El primer ensayo en el suelo falló de una forma que el banco de pruebas no habría podido mostrar. En el aire se habían superado todas las comprobaciones: las ruedas giraban, los encoders contaban, el sensor leía la inclinación. En el suelo, las ruedas se alejaban de la inclinación en lugar de meterse debajo de ella, y el robot se cayó. El ángulo estaba bien; lo que estaba invertido era el «adelante» de los motores y de los encoders. Tras invertir ambos en la configuración, el robot se puso de pie, y con estas ganancias más tarde aguantó más de dos minutos con una desviación estándar de su ángulo de 0,2°.

Aquí es donde un solo lazo llega a su límite. Durante ese empujón el cuerpo nunca se inclinó más de medio grado, y el robot rodó 27 cm y se quedó ahí. Para un lazo de ángulo eso es un éxito, porque el ángulo está bien. No distingue entre estar quieto y rodar a velocidad constante: en nuestro suelo, mantener una velocidad v cuesta alrededor del 4,5 % de la potencia más un 78 % por cada metro por segundo, y la integral simplemente conserva la potencia que le haya dejado el empujón. Cualquier velocidad le parece equilibrio.

Segundo lazo: la velocidad

El segundo lazo vigila las ruedas. Se ejecuta 100 veces por segundo, lee los dos encoders y compara la velocidad del robot con la que debería tener, por ahora cero. No toca los motores en absoluto: su salida es el ángulo que debe mantener el primer lazo. ¿Va rodando hacia adelante? Que se incline un poco hacia atrás, y el lazo de ángulo, al perseguir el nuevo ángulo, frena el robot. Es un controlador PI, con una integral limitada a los mismos pocos grados que la salida:

float pidSpeed(float vRef, float v, const Params &p, float dt,
               bool hold) {
    const float error = vRef - v;
    const float summed = hold ? speedIntegral
                              : speedIntegral + p.kvi * error * dt;
    speedIntegral = constrain(summed, -p.thetaMax, p.thetaMax);
    return constrain(p.kv * error + speedIntegral,
                     -p.thetaMax, p.thetaMax);
}

El primer intento temblaba cinco veces por segundo. Su ganancia salió de un modelo que suponía un lazo de ángulo tres veces más débil que el real: kv = 10 se calculó para kp = 10, y el robot funciona con kp = 30. El modelo también ignoraba la décima de segundo que tardan las ruedas en responder. La solución no fue una ganancia menor, sino una medición más tranquila: ahora la velocidad se promedia en 200 ms en lugar de 40 ms, y da la casualidad de que un promedio de 200 ms anula por completo una oscilación de 5 Hz.

Luego el robot se escapaba poco a poco. Con un lazo de velocidad puramente proporcional, kvi = 0, ocho de cada diez ensayos terminaron a más de 10 cm de donde habían empezado. Una integral pequeña, kvi = 1, lo arregló: en los once ensayos siguientes ninguno se desplazó más de 2,7 cm.

Y entonces hizo algo que nadie le había pedido: tras un empujón, volvía.

La razón está en la integral. La integral de la velocidad es simplemente la distancia recorrida, así que, para mantener a velocidad cero un robot desplazado, la integral de la velocidad acaba sosteniendo una pequeña inclinación, de alrededor de un grado por cada metro de distancia al punto de partida. Es un resorte débil y oculto que tira del robot hacia casa. Y débil es la palabra: el robot vuelve a una franja de unos 30 cm de ancho y no a un punto, y en esta toma dos de sus puntos de reposo quedaron a 34 cm uno del otro. Suponemos que dentro de la franja la inclinación es demasiado pequeña para vencer la holgura de la reductora, pero es una suposición, no una medición. La toma de arriba se hizo con kv = 5; la etapa se cerró con kv = 7.

Tercer lazo: la posición

El tercer lazo recuerda dónde empezó el robot, y marcamos ese punto con un barquito impreso. Veinte veces por segundo, el lazo mide la distancia hasta casa y le pide al lazo de velocidad una velocidad de vuelta. Su ganancia es kx = 1,5, así que a diez centímetros de casa pediría 15 cm/s, pero nunca pide más de 10 cm/s. La posición fija la velocidad, la velocidad fija el ángulo y el ángulo fija los motores.

La distancia sale de los encoders, corregida teniendo en cuenta la inclinación: cuando el cuerpo se inclina, las ruedas avanzan por debajo de él aunque el robot en su conjunto no se mueva. Esta es la cascada tal como se ejecuta dentro del paso de 500 Hz, un poco abreviada: se ha omitido la rama que permite al PC conducir el robot.

// Every step runs at 500 Hz: position every 25th, speed every 5th.
if (positionPhase == 0) {
    vRefNow = pidPosition(xHold + params.xRef, xNow, params);
}
positionPhase = (positionPhase + 1) % POSITION_EVERY;

if (speedPhase == 0) {
    // Hold the speed integral while a hand carries the robot away.
    const bool limited = fabsf(vRefNow) >= params.vMax;
    const bool carried = (vRefNow > 0) ? (vNow < -AWAY_M_S)
                                       : (vNow > AWAY_M_S);
    thetaOffset = pidSpeed(vRefNow, vNow, params,
                           SPEED_EVERY * IMU_DT, limited && carried);
}
speedPhase = (speedPhase + 1) % SPEED_EVERY;

thetaRefNow = params.thetaRef + thetaOffset;
percent = pidStep(a.theta, a.omegaPitch, thetaRefNow, params,
                  IMU_DT, &uCtl);

La línea «carried» («llevado») tiene su historia. En la primera versión, el robot volvía tras un empujón a velocidades de entre 20 y 46 cm/s y se pasaba de su sitio de 10 a 24 cm. La integral de la velocidad se había ido cargando mientras empujaba la mano, y recordaba la mano. Congelar la integral siempre que el lazo de posición pedía la velocidad máxima redujo el sobrepaso a unos 5 cm, pero entonces los empujones grandes terminaban de 12 a 27 cm antes de llegar a casa. La regla con la que nos quedamos la congela solo mientras el lazo pide la velocidad máxima de vuelta y el robot sigue alejándose de casa a más de 1 cm/s, es decir, mientras una mano lo lleva. A partir de ahí, tras empujones de 68 y 80 cm, el robot volvió a menos de 3 cm, sin sobrepaso.

El propio kx se eligió moviendo el punto objetivo por Wi-Fi y observando cómo lo seguía el robot, con 36 desplazamientos para cada valor:

No vuelve exactamente al barquito. Se detiene dos o tres centímetros antes o después de él, retenido por la misma fricción que permitía al robot de dos lazos detenerse en cualquier punto de su franja. Deshacer un empujón de 50 cm en el sentido contrario le llevó 8,6 segundos. Si pones las tres etapas una al lado de otra, el progreso salta a la vista:

Conducirlo por Wi-Fi

Una vez que el robot sabe mantenerse en un punto, conducirlo es cuestión de mover ese punto. El ESP32 se conecta a la red Wi-Fi de casa, y un pequeño puente en Python en el PC sirve una página web con flechas: teclado, mando o un mando en pantalla. El puente reenvía la orden más reciente al robot por UDP veinte veces por segundo, y el robot responde al instante a cada orden con su telemetría: ángulo, velocidad, posición, rumbo, potencia, voltaje de la batería e intensidad de la señal. Mientras se mantiene pulsado un control, una velocidad de marcha con rampa suave sustituye a la salida del lazo de posición; al soltarlo, el lazo de posición mantiene el punto donde se ha detenido el robot.

Para girar hizo falta un lazo más, fuera de la cascada. Un lazo de rumbo compara la diferencia entre los encoders de las dos ruedas con una consigna que se mueve a la velocidad de giro pedida, y luego suma su corrección a una rueda y la resta de la otra. El mismo lazo impide que el robot gire por sí solo a causa de unos motores desiguales. La regla es que el equilibrio va primero: el giro solo recibe la potencia que le sobra al lazo de ángulo.

// Balance first: the turn gets what is left of the limit, so the
// mean of the two wheels stays the angle loop's command.
static void driveApart(float percent, float turn, float limit) {
    drivePercent = percent;
    const float room = max(limit - fabsf(percent), 0.0f);
    uTurnNow = constrain(turn, -room, room);
    motorDrive(Wheel::Left, percent - uTurnNow);
    motorDrive(Wheel::Right, percent + uTurnNow);
}

Los límites de trabajo son 0,35 m/s y 75° por segundo. El último ensayo del 8 de noviembre duró 150 segundos: 28 metros de conducción con picos de hasta 0,57 m/s, dos saltos desde la báscula y ninguna caída. En el primer aterrizaje el cuerpo se inclinó por un instante hasta +7°, y el robot siguió de pie.

No todos los saltos salieron tan bien. Ese mismo día, más temprano, un aterrizaje desde una pequeña rampa inclinó el robot 9° hacia adelante. Para no caerse, salió disparado a cerca de un metro por segundo y fue directo contra la cama. Para que el robot se incline hacia atrás y frene, las ruedas primero tienen que adelantarse al cuerpo, y la orden de potencia ya estaba en su límite del 85 %: a esa velocidad no quedaba margen para llevarlas hasta ahí. Desde entonces el límite se ha subido al 100 %, algo que todavía no hemos probado en el suelo.

Qué evita que se haga daño

• Solo arranca cuando se le pide. Al pulsar un botón se arma con tres pitidos, y se activa cuando su inclinación se mantiene a menos de 5° de la vertical durante 0,2 s.
• Se rinde a los 25°. Más allá de esa inclinación ya no puede recuperarse, así que los motores se apagan en lugar de hacer girar las ruedas contra el suelo.
• Un watchdog propio. Un temporizador por hardware aparte desactiva la línea de habilitación (enable) de los drivers si no ha llegado ninguna muestra del sensor en 10 ms. Un bus I²C colgado puede bloquear la tarea de control durante cerca de un segundo, y un robot que va a ciegas durante un segundo es un robot empotrado en la pared.
• Comprobaciones de batería y sensor. Se detiene si la batería está baja o si falla el sensor. Si se pierde el enlace Wi-Fi, solo se detiene la conducción: el robot se queda quieto y sigue en equilibrio.

Mira el montaje completo

El vídeo cuenta la misma historia en ocho minutos y medio, con los registros dibujados sobre las imágenes. El reproductor se carga desde YouTube solo cuando lo pulsas.

Reproducir el vídeo: Construí un robot autoequilibrado: tres lazos de control 8:40

Construí un robot autoequilibrado: tres lazos de control

• Ver en YouTube

Al pulsar reproducir, el vídeo se carga desde YouTube. Política de privacidad

Próximos pasos

Diez días, tres lazos, un robot que se mantiene de pie. La cascada PID funciona, pero es una pila de piezas ajustadas a mano, y cada pieza se añadió para resolver lo que la anterior no sabía hacer.

Lo próximo que queremos es sustituir los tres lazos por un único controlador LQR, que calcula sus ganancias a partir de un modelo del robot, en lugar de que una persona las ajuste una a una. Después queremos probar una pequeña red neuronal que se ejecute en el propio ESP32. El robot ya registra sus ensayos, en parte para que una red así tenga de qué aprender. Todavía no existe ninguno de los dos. Cuando existan, se enfrentarán a los mismos empujones que aquí recibió el PID, y publicaremos las cifras.