Simulador de robô autoequilibrado com PID

Controles

Iniciar, Pausar e Reiniciar executam o ciclo de controle. Empurrar aplica um impulso como um dedo faria, e Ligar/desligar o controlador desativa ambos os controladores para que você veja a planta sem controle cair.

O controle mais direto é o próprio robô: arraste o corpo até qualquer ângulo e solte. A pose em que você o deixar é a pose a partir da qual o controlador assume o comando — não há nenhuma etapa de rearmamento, e os controles de ganho se aplicam imediatamente à simulação em andamento. Zere o ganho derivativo enquanto ele está equilibrando e ele começará a oscilar no mesmo segundo.

O menu Ajuste carrega quatro predefinições: uma que funciona e três que falham nas três maneiras pelas quais os equilibradores reais falham.

Mantenha o canvas em foco e use Espaço, S, L, R e F para acessar os mesmos controles pelo teclado.

Teoria

Manter um robô sobre duas rodas significa manter um pêndulo invertido na vertical. O centro de massa fica acima do eixo, portanto a vertical é um equilíbrio no mesmo sentido em que um lápis equilibrado sobre a ponta está: matematicamente real, mas fisicamente impraticável sem correção constante. Incline-o em qualquer ângulo e a gravidade produz um torque que o inclina ainda mais. Quanto mais ele avança, mais rápido avança.

O robô tem exatamente uma maneira de combater esse torque — pode mover o eixo. Acelerar as rodas para frente empurra a base de volta para baixo do centro de massa, o que gira o corpo de volta em direção à vertical. É por isso que a correção parece ter sinal invertido no início: para parar de cair para frente, você deve avançar, da mesma forma que move a mão sob uma vassoura que está equilibrando, em vez de movê-la para longe.

Isso dá ao robô um limite rígido. Além de aproximadamente 35°, a aceleração horizontal necessária supera o que as rodas conseguem entregar antes de o corpo atingir o piso, e nenhum ganho ajuda. As linhas tracejadas no traçado marcam esse limite.

Algoritmo

O controlador é em cascata — dois controladores PID, um alimentando o outro.

O controlador interno é rápido e mantém um ângulo de inclinação. Ele compara o ângulo medido com um alvo e comanda o torque dos motores:

error   = measured_angle − target_angle
motor   = Kp·error + Ki·∫error + Kd·d(measured)/dt

Observe que o erro é a medição menos o alvo, e não o alvo menos a medição, como manda o livro. Interceptar uma queda para frente significa acionar as rodas para frente, portanto uma inclinação positiva precisa produzir um comando positivo para o motor. Esse sinal é o motivo mais comum para um primeiro robô equilibrador se lançar contra o piso assim que é ligado.

O controlador externo é mais lento e mantém uma posição. Ele não pode comandar os motores — eles já estão comprometidos — portanto, em vez disso, comanda um alvo de inclinação para o controlador interno:

target_angle = Kp_pos·(desired_position − position) − Kd_pos·velocity

Quando está atrás da posição desejada, solicita uma pequena inclinação para frente; o controlador interno então move as rodas para manter essa inclinação, e o robô avança. A inclinação é deliberadamente limitada a poucos graus. Um controlador externo autorizado a exigir um ângulo grande entrega ao controlador interno um alvo que ele não consegue manter, e ambos perdem ao mesmo tempo.

O termo integral importa mais aqui do que na maioria dos controladores, porque é ele que absorve a diferença entre onde a IMU considera a vertical e onde a vertical realmente está. Também é por isso que o integrador precisa de proteção contra windup: enquanto os motores estiverem saturados, ele deve interromper a acumulação do erro. Sem essa proteção, ele se acumula durante uma queda que não consegue impedir e depois briga com a recuperação.

Código-fonte

// Inner loop: tilt → motors. Runs every 5 ms off a timer, not in loop().
float angleError = angle - tiltTarget;          // measured minus target
angleIntegral   = constrain(angleIntegral + angleError * dt, -3, 3);
float command   = Kp * angleError
                + Ki * angleIntegral
                + Kd * gyroRate;                // rate straight from the gyro

command = constrain(command, -255, 255);
if (fabs(angle) > FALL_ANGLE) command = 0;      // past saving; cut the motors
driveMotors(command, command);

// Outer loop: position → tilt target. Runs ten times slower, and may only
// ever ask for a small lean.
if (++slowTick >= 10) {
  slowTick = 0;
  float positionError = 0 - wheelPosition;
  tiltTarget = constrain(KpPos * positionError - KdPos * wheelVelocity,
                         -0.12, 0.12);          // radians, about ±7°
}

Dois detalhes do simulador se transferem diretamente. Tire o termo derivativo da taxa do giroscópio, em vez de diferenciar o ângulo: o giroscópio mede essa taxa diretamente e sem o ruído que a diferenciação amplifica. E corte os motores além do ângulo de recuperação, em vez de deixar o controlador continuar se esforçando — isso preserva as caixas de engrenagem e torna a falha evidente, em vez de violenta.

Hardware

O sensor é uma MPU6050, que contém um acelerômetro e um giroscópio, porque nenhum dos dois é utilizável sozinho. O acelerômetro indica a direção da gravidade, mas fica contaminado pelo movimento do próprio robô; o giroscópio acompanha a rotação de forma limpa, mas deriva em poucos segundos. Um filtro complementar funde as duas medições em um ângulo estável e rápido.

Os motores precisam ter uma relação de redução baixa o suficiente para fornecer torque e girar rápido o suficiente para recuperar uma queda — motorredutores N20 em torno de 200 RPM são uma escolha comum, acionados por um TB6612FNG, cuja baixa queda de tensão importa quando o controlador solicita pequenas correções milhares de vezes por segundo.

Construa o projeto real no projeto do robô autoequilibrado ou comece pelo tutorial de leitura de inclinação se a IMU for nova para você.

Premissas e diferenças do hardware

O modelo é um pêndulo invertido 2D com um único eixo de inclinação, elos rígidos, sem derrapagem das rodas e uma IMU que informa o ângulo e a taxa verdadeiros, apenas com a deriva e o ruído que você definir. Um robô equilibrador real sente um torque de motor que varia com a tensão da bateria, a quantização do encoder, o deslocamento da montagem da IMU e a deriva de temperatura, e também pode cair para o lado — um grau de liberdade que este laboratório elimina por completo. A estrutura PID em cascata e o termo derivativo baseado na taxa do giroscópio mostrados aqui são transferidos diretamente, mas o ajuste de montagem, o ajuste do filtro e o corte no ângulo de queda precisam ser determinados novamente no chassi físico.

Diagrama do circuito

Duas regras de fiação decidem se este robô funciona.

A bateria do motor deve alcançar o pino VM do driver e nada mais. A corrente do motor nunca deve passar pelo Arduino, e todos os terras — placa, driver, IMU e negativo da bateria — precisam estar conectados, ou os sinais de controle não terão uma referência comum e a falha será silenciosa, não evidente.

O encoder deve ser alimentado pela lógica, não pelo trilho do motor. Alimentar um encoder com a tensão do motor destrói o encoder ou enche as contagens com ruído de comutação. Coloque também o canal A do encoder em um pino capaz de gerar interrupção; consultá-lo a partir de loop() perde contagens, e contagens perdidas significam que o controlador externo perde lentamente de vista a posição do robô.

Monte a IMU perto do eixo. Montá-la muito alto amplifica a aceleração linear que ela mede durante cada correção, o que torna o problema de fusão mais difícil sem qualquer benefício.