Seguimento de parede e curvas precisas para um robô de labirinto

Introdução

Um algoritmo de resolução de labirintos supõe que o robô está na célula em que pensa estar. O preenchimento por inundação calcula uma rota perfeita e, então, o robô corta uma parede na terceira curva; todas as decisões seguintes passam a ser tomadas sobre a célula errada.

Na maioria dos casos, portanto, o algoritmo não é o que falha. O que falha é o movimento: permanecer centralizado em um corredor e girar exatamente noventa graus. Este tutorial trata desses dois pontos e de por que a abordagem óbvia para cada um é a errada.

Por que a navegação por estimativa sozinha não basta

A abordagem tentadora é avançar apenas com base em contagens de encoder: uma célula corresponde a N contagens, uma volta de 90° corresponde a M contagens e o labirinto é uma grade; portanto, basta contar.

Ela falha porque os erros se acumulam e nunca são autocorrigidos. As rodas patinam durante a aceleração. Os dois motores não são idênticos. A distância entre eixos que você mediu está errada em um milímetro, e cada curva termina em 88° em vez de 90°. Cada erro é pequeno; nenhum deles se cancela. Após vinte células, o robô avança na diagonal em direção às paredes com total confiança.

A solução é usar o próprio labirinto como referência. As paredes são retas, paralelas e têm espaçamento conhecido — uma medição gratuita, à prova de deriva e disponível continuamente.

Seguimento de parede como um laço PID

Com um sensor de distância em cada lado, centralizar é um problema de controle que você já resolveu uma vez no seguidor de linha: calcule um erro e leve-o a zero.

// Positive error = too close to the left wall = steer right.
float error = leftDistance - rightDistance;

float correction = Kp * error + Kd * (error - previousError);
previousError = error;

int base = 120;
setMotors(base + correction, base - correction);

A correção é aplicada diferencialmente — somada a uma roda e subtraída da outra — para que a velocidade para a frente do robô permaneça aproximadamente constante enquanto sua orientação muda. Essa é a mesma estrutura de um seguidor de linha; apenas o termo de erro é diferente.

Comece com Kd em zero, aumente Kp até o robô acompanhar o corredor enquanto oscila levemente e depois acrescente Kd até a oscilação ser amortecida. Ki quase nunca é útil aqui: um corredor de labirinto é curto, e a ação integral geralmente apenas acrescenta atraso e excedente em cada limite de célula.

O caso que o laço ingênuo resolve errado

Um labirinto tem aberturas. No instante em que uma parede lateral termina, aquele sensor passa a indicar uma distância grande — até a parede oposta de uma célula aberta, ou até nada — e leftDistance - rightDistance se torna um erro enorme e falso. O robô desvia bruscamente para a abertura que deveria apenas atravessar.

Verifique a presença da parede antes de confiar na diferença:

const int WALL_THRESHOLD = 100;   // mm — beyond this there is no wall

bool leftWall  = leftDistance  < WALL_THRESHOLD;
bool rightWall = rightDistance < WALL_THRESHOLD;

float error;
if (leftWall && rightWall) {
  error = leftDistance - rightDistance;        // centre between both
} else if (leftWall) {
  error = 2 * (leftDistance - IDEAL_SIDE);     // hold one wall at a set distance
} else if (rightWall) {
  error = 2 * (IDEAL_SIDE - rightDistance);
} else {
  error = 0;                                    // open cell — hold heading instead
}

Os casos com parede em apenas um lado dobram o ganho porque uma única parede oferece metade da informação: com duas paredes, derivar 5 mm para a esquerda altera a diferença em 10 mm; com uma, altera em 5.

No caso sem parede, recorrer a error = 0 faz o robô avançar em linha reta do ponto de vista dos motores — o que não é linha reta na realidade. É aí que o giroscópio assume.

Girando exatamente 90 graus

Contar pulsos do encoder durante um giro é a primeira tentativa natural, mas não é precisa o bastante. Um giro no lugar patina mais do que o movimento para a frente, e um erro de 2° por curva se acumula e chega a 20° após dez curvas — deixando o robô diagonalmente voltado para o fundo do corredor.

Integre o giroscópio. A taxa de rotação do eixo Z do MPU6050 mede diretamente a velocidade angular do robô e não depende de as rodas terem patinado:

float turnDegrees(float target) {
  float heading = 0;
  unsigned long last = micros();

  while (fabs(heading) < fabs(target)) {
    unsigned long now = micros();
    float dt = (now - last) / 1000000.0;
    last = now;

    heading += (readGyroZ() - gyroBias) * dt;   // bias subtraction is essential

    int speed = (target > 0) ? TURN_SPEED : -TURN_SPEED;
    setMotors(speed, -speed);                   // pivot in place
  }
  setMotors(0, 0);
  return heading;                               // report the real angle achieved
}

Três detalhes são decisivos:

Subtraia o viés do giroscópio. Um giroscópio não calibrado indica um ou dois graus por segundo enquanto está parado, e você está integrando esse valor. Meça a média enquanto ele está imóvel, exatamente como no tutorial de inclinação, e subtraia-a de cada leitura.

Freie antes e deixe a inércia completar o giro. O robô possui momento angular; portanto, cortar os motores exatamente aos 90° produz um excedente. Comece a frear por volta de 85° e deixe o robô completar o movimento por inércia — ou, ainda melhor, reduza gradualmente a velocidade de giro à medida que o ângulo restante diminui.

Alinhe-se à parede dianteira. Quando uma curva coloca uma parede na frente do robô, essa parede é uma referência perfeita de 90°. Aproxime-se lentamente e deixe o laço de seguimento de parede alinhar o robô a ela, o que zera gratuitamente o erro de orientação acumulado. Quem constrói robôs micromouse faz isso sempre que pode, porque transforma um robô que deriva em um robô que se autocorrige.

Saber quando uma célula ficou para trás

A contagem de células é o que conecta o movimento ao algoritmo do labirinto. A distância medida pelos encoders leva aproximadamente até lá; as paredes informam exatamente.

O gatilho mais confiável é a transição de queda de uma parede lateral. À medida que o robô deixa uma célula para trás, um sensor lateral que indicava uma parede passa a indicar uma distância grande — essa transição ocorre em um ponto conhecido da célula e representa um marco físico, não uma estimativa acumulada.

Combine as duas fontes: use os encoders para prever o limite e deixe a borda da parede confirmá-lo ou corrigi-lo. Quando divergirem, acredite na parede.

Verifique antes de deixá-lo resolver

Quatro testes, no labirinto real, nesta ordem:

1. Corredor reto, dez células. O robô deve permanecer centralizado e chegar ainda centralizado. Oscilação indica que Kd está baixo; desvio para um lado indica um desalinhamento do sensor — meça os dois sensores laterais em relação à mesma parede e corrija a diferença em software.
2. Corredor com parede em apenas um lado. Remova uma parede. O robô deve continuar em frente, em vez de desviar bruscamente para a abertura.
3. Dez curvas seguidas. Gire 90° dez vezes e verifique se o robô termina orientado na direção original. Um desvio de mais de alguns graus indica que o viés do giroscópio ou a redução da velocidade do giro precisa ser ajustada.
4. Alinhamento. Dirija o robô contra uma parede, deixe-o se alinhar e confirme que o erro de orientação desapareceu depois.

Só quando esses quatro testes passarem vale executar um algoritmo de labirinto por cima — um planejador que recebe dados ruins de posição produz certezas falsas. Observe a camada de planejamento isoladamente no simulador de labirintos e depois junte as duas coisas no projeto micromouse.