Controle PID em cascata para um robô autobalanceador

Introdução

Você já dispõe de um ângulo de inclinação confiável. Agora precisa manter o robô apoiado nesse ângulo — e, depois, fazer com que ele permaneça no lugar, em vez de derivar pelo ambiente enquanto permanece tecnicamente ereto.

Essas são duas tarefas diferentes, e o erro que quase todo mundo comete primeiro é tentar realizá-las com um único laço. Este tutorial constrói dois laços aninhados, que é como robôs autobalanceadores reais são controlados.

Por que um único laço não funciona

O que você quer é um robô que fique onde o colocou. O que você pode comandar é o torque dos motores. Entre os dois há um fato inconveniente: a única maneira de mudar a posição do robô é mudar primeiro sua inclinação.

Tente controlar a posição diretamente — acione os motores proporcionalmente ao erro de posição — e as rodas sairão de baixo do centro de massa. O corpo gira no sentido oposto e o robô cai imediatamente, tendo se movido na direção errada durante a queda. Isso não é um problema de ajuste; o sinal da resposta inicial é realmente invertido, uma propriedade chamada fase não mínima.

A solução é controlar as duas grandezas em velocidades diferentes, em cascata.

A estrutura

   position error → [ OUTER PID ] → tilt target
                                        ↓
   tilt error     → [ INNER PID ] → motor command → robot
                                        ↑
                                   measured tilt

O laço interno faz uma tarefa: manter o ângulo de inclinação solicitado. Ele funciona rapidamente — a 200 Hz ou mais — porque está lutando contra a gravidade, e a gravidade não espera.

O laço externo faz a outra tarefa: levar o robô a uma posição. Ele não pode acionar diretamente os motores, portanto solicita ao laço interno uma pequena inclinação. Quando o robô está atrás da posição desejada, o laço externo solicita uma inclinação para a frente; o laço interno aciona as rodas para manter essa inclinação; manter uma inclinação para a frente significa acelerar continuamente para a frente. Ele funciona talvez dez vezes mais devagar que o laço interno.

Essa separação entre as taxas de execução é o princípio de todo o projeto. Se o laço externo operar em uma velocidade próxima à do interno, eles entram em conflito — o laço externo altera o alvo mais rápido do que o interno consegue acompanhar, e o robô se faz oscilar até cair no chão.

O laço 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);

Três detalhes aí importam mais do que os ganhos.

O erro é medido menos o alvo, o oposto do setpoint − measurement do modelo clássico. Para detectar que o robô está caindo para a frente, mova as rodas para a frente, sob a massa em queda, da mesma forma que você apoia a mão sob uma vassoura. Portanto, uma inclinação positiva deve produzir um comando positivo para os motores. Inverter esse sinal faz o controlador empurrar o robô contra o chão no instante em que ele é ligado — uma falha distinta e violenta, quase sempre causada por esse erro de sinal.

O termo derivativo usa diretamente a taxa angular medida pelo giroscópio, não a diferença entre ângulos sucessivos. O giroscópio mede a taxa angular como sua saída natural. Diferenciar um sinal de ângulo ruidoso amplifica exatamente o ruído que menos se quer amplificar, e essa é a razão pela qual muitas primeiras tentativas precisam de um ganho D suspeitamente baixo para permanecer estáveis.

O termo integral é limitado. Durante uma queda, os motores saturam e o erro continua crescendo; sem limite, o termo integral cresce até um valor enorme e depois se opõe à recuperação quando o robô volta. Limite-o e interrompa totalmente a acumulação enquanto a saída estiver saturada.

O laço 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 vem da soma das contagens do seu encoder. Se você não tiver encoders, pode integrar o comando dos motores como uma aproximação rudimentar, mas o laço ficará perceptivelmente pior.

O limite não é opcional. Um laço externo que pode solicitar 30° fornece ao laço interno um alvo que ele não consegue manter fisicamente — o robô assume uma inclinação da qual não consegue se recuperar e cai enquanto cumpre a referência à perfeição. Poucos graus são suficientes; uma inclinação de 7° acelera um robô com rapidez.

Observe que o termo de velocidade é subtraído. Ele amortece a aproximação para que o robô não ultrapasse o alvo e precise retornar.

Ajuste na ordem que funciona

Ajuste primeiro o laço interno sozinho. Defina KpPos e KdPos como zero para que o laço externo não tenha influência e tiltTarget permaneça em zero. O robô manterá o equilíbrio no lugar e derivará lentamente — isso é esperado e está tudo bem por enquanto.

1. Defina Ki e Kd como zero. Aumente Kp até o robô reagir bruscamente a um empurrão. Se for baixo demais, ele cede até o chão sem realmente resistir; se estiver alto demais, vibra.
2. Aumente Kd até a oscilação ser amortecida. Este é o termo que transforma um robô que se faz vibrar até cair em um robô que se estabiliza. No simulador de equilíbrio você pode definir D como zero em um robô em funcionamento e contar as oscilações antes que ele caia.
3. Aumente Ki por último, e com cuidado. Ele elimina a inclinação lenta causada pelo ajuste de montagem da IMU. Se for demais, o robô desenvolve uma oscilação lenta e crescente à medida que o termo integral se acumula em excesso e depois é descarregado.

Só então habilite o laço externo e comece KpPos em cerca de um décimo do que parecer razoável. O efeito é indireto — você observa o robô parar de derivar, não uma resposta rápida.

Modos de falha e o que significam

Desligue os motores quando a recuperação não for mais possível

if (fabs(angle) > FALL_ANGLE) {             // about 35 degrees
  driveMotors(0, 0);
  angleIntegral = 0;
  return;
}

Passado aproximadamente 35°, a aceleração horizontal necessária para fazer as rodas retornarem sob o centro de massa é maior do que elas conseguem fornecer antes de o corpo alcançar o chão. O controlador não sabe disso e tentará usar todo o torque durante a queda, o que desgasta as caixas de engrenagens e torna a falha mais barulhenta do que precisaria ser.

Desligar os motores também zera o termo integral; assim, erguer o robô fornece um laço limpo, em vez de um laço que ainda carrega uma correção acumulada durante a queda.

Para onde seguir

Os ganhos aqui interagem mais entre si do que em um PID de laço único, porque o sistema é instável — não existe um “mais ou menos certo” que apenas funcione mal. Ou o robô fica em pé, ou não fica.

Isso torna um simulador especialmente útil: você pode definir D como zero em um robô que está mantendo o equilíbrio, observar a falha exata e desfazer a alteração sem consertar nada. Experimente as predefinições no simulador de equilíbrio e depois construa o projeto real do robô com autobalanceamento.