Ângulo de inclinação com MPU6050: acelerômetro e giroscópio

Introdução

O MPU6050 é a resposta honesta e mais barata para a pergunta “qual é o lado de cima?”. Ele contém dois sensores independentes no mesmo chip — um acelerômetro de três eixos e um giroscópio de três eixos — e um robô autoequilibrado precisa dos dois, porque cada um é inútil sozinho de uma maneira que o outro compensa exatamente.

Este tutorial leva você de um módulo sem componentes a um ângulo de inclinação em graus confiável enquanto o robô está parado. Para tornar esse ângulo confiável durante o movimento do robô, consulte o filtro complementar, que é o tutorial seguinte e depende de tudo que está aqui.

O que cada sensor realmente mede

Um acelerômetro não mede inclinação. Ele mede aceleração, incluindo a aceleração devido à gravidade, e é por isso que funciona como sensor de inclinação: quando está parado, a única aceleração é 1 g apontando para baixo, e a forma como esse vetor se distribui pelos eixos informa sua orientação.

Esse “estar parado” é a armadilha. O sensor não consegue distinguir a gravidade de qualquer outra aceleração. No instante em que as rodas do robô se movem, o acelerômetro informa a soma vetorial da gravidade e da aceleração inercial produzida pelo movimento, e indica uma inclinação que não existe. Em um robô autoequilibrado — que, por definição, nunca para de acelerar — esse erro é contínuo.

Um giroscópio mede a taxa angular, em graus por segundo. Integre a taxa ao longo do tempo e você terá um ângulo. Giroscópios são imunes à aceleração linear, então continuam honestos enquanto o robô se move.

A fraqueza deles é que a integração acumula erro. Qualquer pequeno viés constante na leitura de taxa se integra a um ângulo que cresce sem limite. Um giroscópio que lê 0,5 °/s enquanto está completamente parado afirmará que você girou 30° após um minuto. Isso é deriva, e não se corrige com uma solda melhor.

Então: o acelerômetro está certo no longo prazo, mas errado a cada instante; o giroscópio está certo a cada instante, mas errado no longo prazo. Essa simetria é toda a razão pela qual a fusão de sensores existe.

Ligação

O MPU6050 comunica-se por I²C, portanto precisa de quatro fios — e, se a varredura não o encontrar em 0x68, aquela página explica por que a folha de dados diz 0xD0.

Em um robô autoequilibrado, monte o módulo de modo que um dos eixos fique realmente horizontal e o mais próximo possível do eixo das rodas. Montá-lo alto multiplica a aceleração linear que ele vê durante uma correção, o que torna o problema de fusão mais difícil sem trazer vantagem alguma.

Lendo valores brutos

Os registradores necessários são consecutivos, então uma única operação de leitura obtém tudo:

#include <Wire.h>

const uint8_t MPU = 0x68;        // 0x69 if AD0 is tied high

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);              // PWR_MGMT_1
  Wire.write(0);                 // clear sleep bit — it boots asleep
  Wire.endTransmission(true);
}

void readRaw(int16_t& ax, int16_t& ay, int16_t& az,
             int16_t& gx, int16_t& gy, int16_t& gz) {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);              // ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)14, (uint8_t)true);

  ax = Wire.read() << 8 | Wire.read();
  ay = Wire.read() << 8 | Wire.read();
  az = Wire.read() << 8 | Wire.read();
  Wire.read(); Wire.read();      // temperature, discarded
  gx = Wire.read() << 8 | Wire.read();
  gy = Wire.read() << 8 | Wire.read();
  gz = Wire.read() << 8 | Wire.read();
}

O módulo inicializa em suspensão. Esquecer de limpar o bit de suspensão em PWR_MGMT_1 retorna um fluxo constante de zeros, que parece exatamente uma falha de ligação e faz as pessoas voltarem à ferramenta de solda sem necessidade.

Convertendo contagens em unidades

Os valores brutos são inteiros com sinal de 16 bits representados como contagens dentro da faixa configurada no sensor. Nos ajustes padrão:

• Acelerômetro: ±2 g em ±32768 contagens → 16384 contagens por g
• Giroscópio: ±250 °/s em ±32768 contagens → 131 contagens por °/s

float accelX = ax / 16384.0;     // g
float rateY  = gy / 131.0;       // degrees per second

Inclinação a partir do acelerômetro

Com o robô em pé e a placa montada verticalmente, o ângulo de inclinação no plano de tombamento do robô vem de dois dos três eixos:

float pitch = atan2(accelX, accelZ) * 180.0 / PI;

Use atan2, não asin de um único eixo. atan2 recebe as duas componentes, então continua correto nos quatro quadrantes e não falha quando um eixo se aproxima de zero. Uma versão de asin em um único eixo parece boa na bancada para ângulos pequenos e depois se comporta de forma estranha quando o robô se inclina além de 45°.

Calibrando os vieses

Todo MPU6050 tem um viés de fabricação. Seu giroscópio não indicará um zero estável enquanto estiver parado, e seu acelerômetro não indicará exatamente 1 g em um eixo e zero nos outros. Meça o erro uma vez na inicialização, com o robô permanecendo parado:

float gyroBiasY = 0;

void calibrate() {
  const int samples = 1000;
  long total = 0;
  for (int i = 0; i < samples; i++) {
    int16_t ax, ay, az, gx, gy, gz;
    readRaw(ax, ay, az, gx, gy, gz);
    total += gy;
    delay(2);
  }
  gyroBiasY = (total / (float)samples) / 131.0;
}

Subtraia gyroBiasY de cada leitura de taxa subsequente. Esse único passo é a diferença entre um giroscópio que deriva um grau por minuto e outro que deriva um grau por segundo.

Há duas coisas a saber. O sensor precisa estar realmente parado durante a calibração — um robô calibrado enquanto você o segura gravará o tremor da sua mão no viés. E o viés muda com a temperatura, então um robô calibrado frio derivará um pouco à medida que os circuitos aquecem. Para um robô autoequilibrado, o termo integral do controle PID em cascata para balanceamento absorve esse resíduo, e essa é uma das razões pelas quais controladores de balanceamento quase sempre precisam de um termo I.

Ajuste de montagem não é viés do sensor

Existe um segundo deslocamento que a calibração não pode remover: sua IMU não está montada perfeitamente. Se a placa estiver 1,5° fora da vertical real, até um sensor perfeitamente calibrado ainda informará 1,5° quando o robô estiver realmente em pé.

Isso importa muito em um robô autoequilibrado e é invisível na bancada. O laço de controle manterá fielmente o ângulo que você disser que é a vertical — então um erro de 1,5° significa que o robô manterá uma inclinação permanente de 1,5°, o que significa aceleração constante, o que significa que ele atravessará a sala até bater em alguma coisa.

Meça isso colocando o robô em pé com a mão, realmente equilibrado, e anotando o ângulo indicado. Subtraia esse número como uma constante. Você pode observar essa falha exata no simulador de balanceamento — o controle de compensação da montagem injeta um erro e o robô se afasta enquanto insiste que está nivelado.

Verifique antes de construir em cima disso

Imprima o ângulo e verifique três coisas manualmente:

1. Em pé lê perto de zero. Se não, há um ajuste de montagem para subtrair.
2. Inclinar o robô para a frente aumenta o ângulo, e para trás o diminui. Se o sinal estiver invertido, troque-o aqui em vez de compensar no laço de controle, onde isso vai confundir você depois.
3. A leitura é estável quando o robô está parado e imediata quando ele se move. Se houver ruído enquanto o robô está parado, verifique a ligação e a alimentação; se houver atraso, você está fazendo média demais em algum lugar.

Quando o ângulo for confiável em repouso, você estará pronto para torná-lo confiável em movimento com o filtro complementar e então fechar o laço no controle PID em cascata para balanceamento.