Acionamento orientado ao campo: direção sempre para a frente

O problema que você terá em dez segundos

Construa um robô com rodas Mecanum, pegue o controle e empurre o joystick para a frente. O robô se afasta de você. Bom.

Agora gire o robô 180° e empurre o joystick para a frente novamente. O robô vem na sua direção, porque frente significa frente para o robô, e ele agora está voltado para você. Por um ou dois segundos, todo operador inverte mentalmente a direção, erra, e o robô atinge alguma coisa.

Isso já é ruim em um robô de acionamento diferencial, em que pelo menos o robô sempre se desloca ao longo do próprio eixo frontal. Em uma base holonômica, é muito pior: o robô pode estar orientado para uma direção e se mover em outra, sem nenhuma pista visual que indique qual orientação lateral o comando acabou assumindo. Após dez segundos de giros, ninguém na sala consegue controlá-lo.

A solução é uma matriz de rotação.

Dois referenciais, uma transformação

Há duas maneiras de interpretar o joystick:

• Orientado ao robô (centrado no robô, relativo ao robô). O joystick é um comando no referencial próprio do robô — base_link, se você conhece ROS. Empurre para a frente e o robô se moverá ao longo do eixo frontal.
• Orientado ao campo (centrado no campo, relativo ao campo). O joystick é um comando no referencial do piso. Empurre para a frente e o robô se moverá para o norte, qualquer que seja a direção do eixo frontal.

O conjunto motriz só compreende o referencial do robô — é nele que a cinemática Mecanum está expressa. Portanto, o controle orientado ao campo exige girar o comando do operador para o referencial do robô antes que a cinemática o receba.

Se a orientação do robô é θ (medida no sentido anti-horário a partir do eixo x do campo), gire o comando por −θ:

vx_robot = vx_field · cos(θ) + vy_field · sin(θ)
vy_robot = −vx_field · sin(θ) + vy_field · cos(θ)

Subtraia θ, não some θ. Errar o sinal produz um robô que parece funcionar enquanto está orientado para a frente, mas se move exatamente na direção errada depois de girar 90° — uma experiência frustrante de dez minutos.

A componente de rotação não é transformada. ω significa a mesma coisa nos dois referenciais — uma rotação é uma rotação — portanto passa diretamente.

Tudo isso, em código

#include <math.h>

// stickX: forward on the stick, positive = away from the driver (field +x)
// stickY: left on the stick,    positive = to the driver's left (field +y)
// stickW: twist, positive = counter-clockwise
void driveFieldOriented(float stickX, float stickY, float stickW) {
  float theta = imuYawRadians() - headingOffset;   // see "the reset" below

  float c = cosf(theta);
  float s = sinf(theta);
  float vx =  stickX * c + stickY * s;
  float vy = -stickX * s + stickY * c;

  driveMecanum(vx, vy, stickW);   // the inverse kinematics, unchanged
}

São seis linhas, e elas separam um robô que as pessoas conseguem controlar de outro que elas não conseguem. Observe que driveMecanum() nunca fica sabendo de nada disso — a orientação de campo é uma transformação aplicada ao comando, não uma alteração no conjunto motriz.

Como obter θ

Você precisa de um ângulo de guinada, e há três maneiras de obtê-lo, em ordem crescente de tempo pelo qual continuam confiáveis.

A guinada de um giroscópio é uma integral, portanto o viés se converte linearmente em deriva:

// Integrate yaw rate, with the bias you measured while it sat still.
float rate = gyroZ() - gyroBiasZ;             // rad/s
if (fabsf(rate) < 0.004f) rate = 0.0f;        // deadband: kill the last of the creep
yaw += rate * dt;

Dois detalhes importam mais do que a escolha do sensor:

• Meça o viés a cada inicialização, com o robô parado. Calcule a média de 500 amostras durante um ou dois segundos antes de qualquer movimento. Um MPU6050 sem correção de viés deriva dezenas de graus por minuto; com a correção, a deriva cai para poucos graus por minuto.
• Use uma zona morta na taxa, não no ângulo. Aplicar zona morta ao ângulo perde curvas reais e lentas. Aplicá-la à taxa descarta apenas o nível de ruído, que é exatamente o que se integra e se transforma em deriva.

Um acelerômetro não ajuda aqui. Ele fornece rolamento e inclinação ao observar a gravidade, e é por isso que um filtro complementar corrige a inclinação tão bem — mas a gravidade é paralela ao eixo de guinada, portanto não informa nada sobre a orientação. A guinada depende de um magnetômetro ou sofre deriva.

A redefinição, que não é opcional

Dois motivos exigem um zero manual:

1. Deriva. Após alguns minutos, a referência de norte do robô e a do operador se separam, e a correção se torna o problema.
2. Orientação inicial. O zero da IMU fica onde o robô estava orientado quando foi ligado, o que raramente corresponde à posição do operador.

Portanto, mantenha um ajuste angular e um botão:

float headingOffset = 0.0f;

void resetFieldHeading() {          // bind to a button the driver can hit blind
  headingOffset = imuYawRadians();  // "whatever you are pointing at is now forward"
}

A convenção adotada por todos é apontar o robô para longe do operador, pressionar o botão e controlá-lo. Leva meio segundo e corrige os dois problemas de uma vez. Todo robô holonômico sério com teleoperação tem esse botão, e toda equipe que o dispensou acabou adicionando-o depois da primeira partida.

Acionamento orientado ao campo também para autonomia

Isso não é apenas um auxiliar de controle. Qualquer comportamento autônomo cujo comando seja expresso naturalmente no referencial do piso precisa da mesma transformação:

• “Mover 300 mm na direção do objetivo” — o objetivo está em coordenadas do campo, portanto o comando também está.
• “Deslocar-se lateralmente para manter o alvo da câmera centralizado enquanto se orbita em torno dele” — a órbita está no referencial do campo, a rotação no referencial do robô, e o controle orientado ao campo permite escrever as duas na mesma linha.
• Tudo o que vem depois de um planejador de caminhos, que trabalha com um mapa e, portanto, no referencial do campo.

No ROS, essa é a transformação odom → base_link realizando o mesmo trabalho. É por isso que TF2 existe e que cmd_vel é definido explicitamente em base_link. O controle orientado ao campo é a implementação direta da transformação que, do contrário, seria delegada ao TF2.

Vendo o resultado

O simulador de acionamento Mecanum tem uma manobra que demonstra o ponto sem necessidade de explicações: mantenha o joystick para a frente, continue girando e mude o referencial.

Mesmo joystick, mesmas rodas, mesma cinemática. A única diferença são seis linhas de trigonometria.

Solução de problemas

A seguir

A orientação de campo faz o comando significar a direção desejada. O controle de velocidade das rodas em laço fechado faz as rodas realmente produzirem o movimento solicitado — em uma base holonômica, as duas funções formam um par, porque um robô cujos motores cedem de forma desigual já se desorienta antes que a IMU possa corrigir.