Robô autoequilibrado com ESP32: três malhas de controlo

Este robô acabou de saltar de uma balança de casa de banho e manteve-se de pé. Cinco dias antes, nem sequer conseguia ficar no mesmo sítio. Entretanto, nada mudou no seu hardware: a mesma estrutura, os mesmos motores, o mesmo sensor. Só mudou o programa. Uma malha de controlo passou a duas, depois a três, e cada nova malha corrigiu o que a anterior não conseguia fazer.

É um robô de duas rodas que se equilibra sozinho: uma estrutura de três andares impressa em 3D, montada sobre dois motorredutores DC, uma placa ESP32 e um sensor de movimento. Construímo-lo para comparar formas de manter um robô de pé com o mesmo hardware, a começar pela clássica, o PID. Este artigo acompanha os dez dias entre o primeiro teste e o salto, pela ordem em que aconteceram: o que tem lá dentro, como é que o robô sabe qual é o lado de cima, as três malhas uma a uma e o que nos ensinou conduzi-lo por Wi-Fi. Os números vêm dos registos do próprio robô.

O robô

A estrutura é feita de três andares impressos, empilhados uns sobre os outros, cada um deles uma peça separada. Desenhámo-la no Fusion 360 juntamente com as peças compradas, para que os motores e os drivers já tivessem o seu lugar antes de se imprimir fosse o que fosse.

De baixo para cima: os motores estão pendurados por baixo do andar inferior, nos suportes que vinham no kit. O primeiro andar tem os dois drivers de motor, com os dissipadores virados para baixo. O segundo andar é o cérebro: a placa ESP32 com o seu pequeno ecrã, montada numa placa perfurada, o sensor de movimento e um buzzer. O andar de cima leva uma bateria de três células com a sua placa de proteção, um interruptor e um módulo redutor de 5 V. O corpo mede 164 × 128 × 60 mm, 235 mm de largura com as rodas, e o robô completo pesa 1025 g.

Dois pormenores da transmissão determinaram tudo o que veio depois. O primeiro é o atraso. O sensor entrega uma nova amostra a cada 2 ms, e o comando que dela resulta chega aos drivers em menos de um milissegundo, mas uma roda precisa de cerca de um décimo de segundo para responder a uma mudança de comando. O segundo é a caixa redutora: tem folga, por isso, depois de cada inversão de sentido, o motor gira durante um instante antes de a roda se mexer.

E os dois motores não são gémeos. Em imagens filmadas ao nível do chão, exatamente o mesmo comando PWM fez girar a roda mais afastada 18 mm e a mais próxima 8 mm. O comando era idêntico; porque é que a resposta não o é, não apurámos. Deixada a si própria, esta diferença faz o robô rodar lentamente no mesmo sítio enquanto se equilibra, e uma pequena correção de rumo, descrita na secção sobre a condução, trata disso.

Qual é o lado de cima

Tudo o que um robô de equilíbrio faz depende de um único número: a sua inclinação. O MPU-6500 oferece duas formas de a obter, e nenhuma funciona sozinha.

O giroscópio mede a rapidez com que o corpo roda. Ao somar as suas leituras a cada 2 ms, obtém-se um ângulo suave e rápido, mas que deriva: sem correção do desvio próprio do giroscópio, medimos cerca de 15° por minuto. O acelerómetro sabe onde está a gravidade, por isso o seu ângulo não pode derivar, mas também sente cada solavanco. Com os motores a trabalhar, o seu ângulo saltava de um lado para o outro numa faixa de 18,7°.

Um filtro complementar fica com a metade boa de cada um. A cada passo, o giroscópio faz avançar o ângulo, e depois o ângulo é puxado em direção à leitura do acelerómetro por uma parte ínfima da diferença, um milésimo. Dito de outra forma: 99,9 % giroscópio, 0,1 % acelerómetro, o que dá uma constante de tempo de dois segundos. Na bancada, o ângulo filtrado manteve-se dentro de seis centésimos de grau.

No firmware, tudo se resume a umas poucas linhas. A placa do sensor está montada ao alto, sobre uma aresta, e inclinada 1,4° para o lado, por isso, antes de mais nada, o código separa a velocidade de arfagem, de que o filtro precisa, da velocidade de guinada, que o código das viragens usa mais tarde:

// 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));

O passo segue o relógio do próprio sensor. Um temporizador de hardware acorda a tarefa de controlo pouco antes de cada amostra esperada, a tarefa lê-a e executa o controlador inteiro, e o ecrã e o registo do ensaio esperam a sua vez com uma prioridade mais baixa. O Wi-Fi vive no outro núcleo. Este esquema vem do nosso balanceador de hélice anterior, construído com a mesma placa e o mesmo sensor.

Primeira malha: o ângulo

A primeira experiência usa uma única malha, que só olha para o ângulo. É um controlador PID executado 500 vezes por segundo, uma vez por cada amostra do sensor, e a sua saída é a potência para os dois motores, em percentagem:

• P, o termo proporcional: kp = 30, ou seja, um grau de erro pede 30 % da potência máxima.
• I, o integral: ki = 30 acumula o erro ao longo do tempo e continua a empurrar enquanto este durar.
• D, o termo derivativo: kd = 0,3 acalma a oscilação. Vai buscar a velocidade de rotação diretamente ao giroscópio, em vez de derivar um ângulo com ruído.

A referência não é exatamente zero. O centro de massa não fica perfeitamente por cima do eixo, por isso o robô equilibra-se em θ_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);
}

O primeiro ensaio no chão falhou de uma forma que a bancada não podia ter mostrado. No ar, todas as verificações tinham passado: as rodas giravam, os encoders contavam, o sensor lia a inclinação. No chão, as rodas andavam para o lado oposto ao da inclinação, em vez de irem para debaixo do corpo, e o robô caiu. O ângulo estava certo; o que estava trocado era a «frente» dos motores e dos encoders. Com ambos invertidos na configuração, o robô pôs-se de pé e, com estes ganhos, mais tarde manteve-se de pé durante mais de dois minutos, com um desvio-padrão do ângulo de 0,2°.

É aqui que uma única malha chega ao limite. Durante esse empurrão, o corpo nunca se inclinou mais de meio grau, e o robô rolou 27 cm e ficou lá. Para uma malha de ângulo, isso é um sucesso, porque o ângulo está bem. Ela não consegue distinguir entre estar parado e rolar a velocidade constante: no nosso chão, manter uma velocidade v exige cerca de 4,5 % de potência mais 78 % por cada metro por segundo, e o integral limita-se a guardar a potência que o empurrão lhe deixou. Para ela, qualquer velocidade parece equilíbrio.

Segunda malha: velocidade

A segunda malha vigia as rodas. É executada 100 vezes por segundo, lê os dois encoders e compara a velocidade do robô com a velocidade que ele devia ter, por agora zero. Não toca de todo nos motores: a sua saída é o ângulo que a primeira malha deve manter. A rolar para a frente? Inclina-se um pouco para trás, e a malha de ângulo, ao perseguir o novo ângulo, trava o robô. É um controlador PI, com um integral limitado aos mesmos poucos graus que a saída:

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);
}

A primeira tentativa tremia cinco vezes por segundo. O seu ganho vinha de um modelo que supunha uma malha de ângulo três vezes mais fraca do que a real: kv = 10 foi calculado para kp = 10, e o robô funciona com kp = 30. O modelo também ignorava o décimo de segundo que as rodas levam a responder. O remédio não foi um ganho mais pequeno, mas uma medição mais calma: a velocidade passou a ser uma média ao longo de 200 ms em vez de 40 ms, e uma média de 200 ms, por acaso, anula por completo uma oscilação de 5 Hz.

Depois, o robô começou a fugir aos poucos. Com uma malha de velocidade puramente proporcional, kvi = 0, oito ensaios em dez terminaram a mais de 10 cm do sítio onde tinham começado. Um pequeno integral, kvi = 1, resolveu o problema: nos onze ensaios seguintes, nenhum se afastou mais de 2,7 cm.

E então fez uma coisa que ninguém lhe tinha pedido: empurrado, voltou.

A razão está no integral. O integral da velocidade é simplesmente a distância percorrida, por isso, para manter a zero a velocidade de um robô deslocado, o integral da velocidade acaba por guardar uma pequena inclinação, cerca de um grau por cada metro de afastamento do ponto inicial. É uma mola escondida e fraca a puxar o robô de volta ao ponto de partida. Fraca é a palavra certa: o robô volta para uma faixa com cerca de 30 cm de largura, e não para um ponto, e neste take dois dos seus pontos de repouso ficaram a 34 cm um do outro. O nosso palpite é que, dentro da faixa, a inclinação é demasiado pequena para fazer a caixa redutora vencer a folga, mas é um palpite, não uma medição. O take acima foi feito com kv = 5; a etapa foi encerrada com kv = 7.

Terceira malha: posição

A terceira malha lembra-se de onde o robô partiu, e marcámos esse sítio com um barquinho impresso. Vinte vezes por segundo, a malha mede a distância ao ponto de partida e pede à malha de velocidade uma velocidade de regresso. O seu ganho é kx = 1,5, por isso, a dez centímetros do ponto de partida, pediria 15 cm/s, mas nunca pede mais de 10 cm/s. A posição define a velocidade, a velocidade define o ângulo, e o ângulo define os motores.

A distância vem dos encoders, corrigida pela inclinação: quando o corpo se inclina, as rodas rolam por baixo dele, mesmo que o robô, no seu todo, fique no mesmo sítio. Eis a cascata tal como é executada dentro do passo de 500 Hz, um pouco encurtada: o ramo que permite ao PC conduzir o robô ficou de fora.

// 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);

A linha «carried» tem uma história por trás. Na primeira versão, depois de um empurrão, o robô regressava a uma velocidade de 20 a 46 cm/s e ultrapassava o seu sítio em 10 a 24 cm. O integral da velocidade tinha estado a acumular-se durante todo o tempo em que a mão empurrava, e lembrava-se da mão. Congelar o integral sempre que a malha de posição pedia a velocidade máxima reduziu a ultrapassagem para cerca de 5 cm, mas então os empurrões grandes passaram a terminar 12 a 27 cm antes do ponto de partida. A regra que ficou só o congela enquanto a malha pede a velocidade máxima de regresso e o robô continua a afastar-se do ponto de partida a mais de 1 cm/s, ou seja, enquanto uma mão o está a levar. A partir daí, depois de empurrões de 68 e 80 cm, o robô regressou a menos de 3 cm do seu sítio, sem ultrapassagem.

O próprio kx foi escolhido ao deslocar o ponto de referência por Wi-Fi e ver o robô segui-lo, 36 deslocamentos para cada valor:

Não volta exatamente ao barquinho. Detém-se dois ou três centímetros antes dele ou depois dele, retido pelo mesmo atrito que deixava o robô de duas malhas assentar em qualquer ponto da sua faixa. Um empurrão de 50 cm no sentido contrário levou 8,6 segundos a ser desfeito. Com as três etapas lado a lado, o progresso salta à vista:

Condução por Wi-Fi

Quando o robô já consegue manter-se num ponto, conduzi-lo é uma questão de mover esse ponto. O ESP32 liga-se à rede Wi-Fi de casa, e uma pequena ponte em Python no PC serve uma página web com setas: teclado, comando de jogos ou comando no ecrã. A ponte reencaminha a ordem mais recente para o robô por UDP vinte vezes por segundo, e o robô responde de imediato a cada ordem com a sua telemetria: ângulo, velocidade, posição, rumo, potência, tensão da bateria e intensidade do sinal. Enquanto um dos controlos está premido, uma velocidade de deslocação que varia em rampa suave toma o lugar da saída da malha de posição; quando é largado, a malha de posição mantém o robô no sítio onde parou.

Para virar, foi necessária mais uma malha, fora da cascata. Uma malha de rumo compara a diferença entre os encoders das duas rodas com uma referência que se desloca à velocidade de viragem pedida e, depois, soma a sua correção a uma roda e subtrai-a à outra. A mesma malha impede o robô de virar sozinho por causa dos motores desiguais. A regra é primeiro o equilíbrio: a viragem só recebe a potência que a malha de ângulo deixa de sobra.

// 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);
}

Os limites de trabalho são 0,35 m/s e 75° por segundo. O último ensaio de 8 de novembro durou 150 segundos: 28 metros de condução com picos até 0,57 m/s, dois saltos da balança e nenhuma queda. Na primeira aterragem, o corpo inclinou-se por um instante até +7°, e o robô manteve-se de pé.

Nem todos os saltos correram tão bem. Mais cedo, nesse mesmo dia, ao aterrar depois de saltar de uma pequena rampa, o robô inclinou-se 9° para a frente. Para recuperar o equilíbrio, arrancou a cerca de um metro por segundo e foi direito à cama. Para o robô se inclinar para trás e travar, as rodas têm primeiro de passar à frente do corpo, e o comando de potência já estava no seu limite de 85 %: àquela velocidade, não sobrava nada para as levar até lá. Entretanto, o limite foi aumentado para 100 %, o que ainda não experimentámos no chão.

O que o impede de se magoar

• Só arranca de propósito. Um toque num botão arma-o com três bipes, e ativa-se depois de a inclinação se manter a menos de 5° da vertical durante 0,2 s.
• Desiste aos 25°. Para lá dessa inclinação já não consegue recuperar, por isso os motores desligam-se em vez de porem as rodas a girar contra o chão.
• Um watchdog próprio. Um temporizador de hardware separado corta a linha de ativação (enable) dos drivers se não chegar nenhuma amostra do sensor durante 10 ms. Um barramento I²C bloqueado pode prender a tarefa de controlo durante cerca de um segundo, e um robô a andar às cegas durante um segundo é um robô contra a parede.
• Verificações da bateria e do sensor. Desliga os motores quando a bateria está fraca e quando o sensor falha. A perda da ligação Wi-Fi só interrompe a condução: o robô imobiliza-se e continua a equilibrar-se.

Veja a montagem completa

O vídeo conta a mesma história em oito minutos e meio, com os registos desenhados por cima das imagens. O leitor só se liga ao YouTube quando carrega nele.

Reproduzir o vídeo: Construí um robô autoequilibrado: três malhas de controlo 8:40

Construí um robô autoequilibrado: três malhas de controlo

• Ver no YouTube

Ao premir o botão de reprodução, o vídeo é carregado a partir do YouTube. Política de privacidade

O que vem a seguir

Dez dias, três malhas, um robô que se mantém de pé. A cascata PID funciona, mas é uma pilha de peças afinadas à mão, e cada peça foi acrescentada para corrigir o que a anterior não conseguia fazer.

A seguir, queremos substituir as três malhas por um único controlador LQR, que calcula os seus ganhos a partir de um modelo do robô, em vez de ser uma pessoa a afiná-los um a um. Depois disso, queremos experimentar uma pequena rede neuronal a correr no próprio ESP32. O robô já guarda registos dos seus ensaios, em parte para que essa rede tenha com que aprender. Nenhum dos dois existe ainda. Quando existirem, vão enfrentar os mesmos empurrões que o PID enfrentou aqui, e vamos publicar os números.