Sensores IMU – uma abordagem completa – Parte 2
Sensores IMU Guia para giroscópio MPU6050
Analisando sistematicamente, o próximo passo no mundo dos MEMS é a interface com o giroscópio, um Sensores IMU. Na maioria dos casos, os dados do giroscópio são fundidos aos dados do acelerômetro. Tendo implementado o acelerômetro antes, percebe-se que o acelerômetro é muito sensível e ruidoso quando se trata da medição da inclinação e rotação (pitch e roll). É possível suavizar os dados através de fusão de sensores. Neste tutorial será abordado quais os dados recebidos pelo giroscópio, como usá-los para calcular inclinação, rotação e guinada e, finalmente, como fundi-los através de um Filtro Complementar.
O Giroscópio
Os giroscópios são utilizados para manter ou para medir orientação. Um giroscópio mecânico normalmente consiste de um disco rotativo, onde os eixos ligados a ele são capazes de se deslocar livremente em qualquer orientação, como na figura 1.
Um giroscópio microeletromecânico (MEMS) é muito semelhante, mas em vez de disco giratório que consiste em um tipo de ressonador vibrando. A ideia é a mesma; um objeto de vibração tende a continuar vibrando no mesmo plano que as suas bases de apoio.
Um giroscópio MEMS mede a velocidade angular (provavelmente graus por segundo), a partir do qual pode-se calcular o ângulo.
Desenvolvimento do Sensores IMU
Assim como no tutorial antecessor a este, sobre o acelerômetro, neste também utilizaremos a Unidade de Medida Inercial (Inertial Measurement Unit, IMU), Sensores IMU, MPU6050 e o Arduino para o processamento dos dados. A comunicação entre os dois continua sendo o protocolo I2C.
As bibliotecas utilizadas aqui são exatamente as mesmas, logo, recorra ao outro post para baixá-las. Ao final deste post será apresentado o código desenvolvido abrangendo tanto a parte do acelerômetro quanto o complemento aqui apresentado sobre o giroscópio e o filtro.
Lendo a velocidade angular bruta e convertendo para graus por segundo (dps)
Assim como na leitura dos valores do acelerômetro, a aquisição dos dados do giroscópio é simplificada com a utilização da biblioteca MPU6050.h e, como os valores não correspondem a nenhuma grandeza física, chamaremos de valores brutos e os aplicaremos em equações para convertê-lo em valores usuais.
Na realização dessa conversão, determina-se o quão rápido o sensor está girando em graus por segundo (dps). Isso é feito multiplicando o valor bruto lido por um constante relacionada ao nível de sensibilidade que foi configurado o giroscópio. Olhando a tabela 1, retirada do datasheet do sensor, relaciona o nível de sensibilidade do sensor à constante a ser multiplicada pelo valor bruto. Vale citar que os valores das constantes devem divididos por 1000. Com isso, se nós escolhemos um nível de sensibilidade de 250 dps na configuração do giroscópio, então temos que multiplicar os valores brutos por 0,131. Um exemplo: Para um nível de sensibilidade de 2000 dps, o ganho será de 0,0164.
Calcular a distância percorrida via integração simples
Neste ponto estamos prontos para converter os dados provenientes do giroscópio em mais algumas informações úteis. Lembrando da física clássica, a distância é obtida pela integração da velocidade:
A ideia é que se tomarmos medições do giroscópio em intervalos de tempo constantes, podemos obter uma estimativa da distância. No entanto, por não ser possível tomarmos uma quantidade infinita de medições do giroscópio, o nosso integrante será apenas uma estimativa, e isso nos levará inevitavelmente a um erro.
Como realizaremos uma medição do giroscópio uma vez por loop do programa, definimos o intervalo de tempo dT como o tempo percorrido durante um loop. Ao aplicar o valor de dT na equação acima, obtemos o valor da distância percorrida, em ângulos.
Combinando dados do acelerômetro com dados giroscópio nos Sensores IMU
Observando as medições do acelerômetro e do giroscópio, Sensores IMU, nota-se que o giroscópio tem leitura muito boa, mas apresenta oscilação. O acelerômetro, por outro lado, é apresenta muito ruído, mas o desvio é zero. A solução para isso é fundir essas duas leituras de sensores para formar uma leitura mais precisa. Existem duas abordagens principais de fusão de dados:
– Filtro de Kalman;
– Filtro Complementar.
Segundo seu criador Rudolph.E. Kalman, o filtro de Kalman é uma solução recursiva para o problema de filtragem de dados discretos em um sistema linear. Dados alguns valores iniciais, pode-se predizer e ajustar os parâmetros do modelo através de cada nova medição, obtendo a estimativa do erro em cada atualização. A sua habilidade para incorporar os efeitos de erros e sua estrutura computacional fez com que o filtro de Kalman tivesse um amplo campo de aplicações, especialmente no que se refere à análise de trajetórias em visão computacional.
O filtro Complementar é uma outra abordagem para essa fusão. Este é muito fácil e intuitivo de implementar que filtro de Kalman e funciona quase tão bem quanto o outro. A ideia do filtro complementar se baseia em definir proporções, ou “pesos”, a cada fonte de dados e somá-los depois. A fórmula geral seria algo como isto:
Onde 0,7, 0,25 e 0,05 são as relações de confiança (ou “pesos”) que tem em uma base de dados. Quanto menor a taxa, menor sua confiança. IMPORTANTE: Note que a soma total das constantes tem de ser 1,0.
No caso de fusão de dados do aceleração e do giroscópio, vamos definir as relações de 0,95 para o giroscópio e 0,05 para o acelerômetro. Este último provavelmente poderia ser ainda mais baixo do que 0,05. Atribuir 0,01 à constante do acelerômetro pode ser ainda melhor. A razão para isso é que queremos ter uma fusão de sensores voltada para o giroscópio, que já é muito lisa e bastante precisa.
Testes de implementação com Sensores IMU
Para teste do código, foi realizada a aquisição de dados dos sensores acelerômetro e giroscópio enquanto aplicava um giro de 90° em sentido horário à unidade inercial. Aplicando todos os cálculos descritos neste artigo e no anterior, foi obtida as curvas abaixo. A curva em azul, descrita como AccXangle na legenda, representa a leitura do acelerômetro com o cálculo da posição do sensor. A curva em vermelho, descrita como CFangleX99, representa o valor final de saída, após a fusão com os dados do giroscópio. Nota-se que foram eliminados os ruídos e que a curva apresenta a mesma forma. Em compensação, foi gerado um pequeno de atraso.
Abaixo é apresentado o código contendo desde a aquisição dos dados do acelerômetro e giroscópio, o tratamento dos dados de ambos sensores e a fusão de dados por filtro complementar. As bibliotecas estão disponibilizadas no primeiro artigo dessa série (link).
[sourcecode language=”c”]
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define G_GAIN 0.00875
#define AA 0.98
float acelx, acely, acelz, rate_gyr_x, rate_gyr_y, rate_gyr_z, gyroXangle, gyroYangle, gyroZangle;
float AccXangle, AccYangle, AccZangle, CFangleX, CFangleY, CFangleZ;
float const_calib = 16071.82;
float const_gravid = 9.81;
unsigned long pT;
void setup() {
Wire.begin(); // Inicia barramento I2C
// initializa conexão serial
Serial.begin(9600);
Serial.println("Initializing I2C devices…");
// verifica conexão com sensores
Serial.println("Testing device connections…");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
unsigned long pT = 0; // contador para determinar tempo de inicialização
}
void loop() {
unsigned long cT = micros(); // contar tempo de loop
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // obtem valores brutos dos sensores
unsigned long dT = cT – pT;
pT = cT;
acelx = ax * const_gravid / const_calib;
acely = ay * const_gravid / const_calib;
acelz = az * const_gravid / const_calib;
// Converte valor do acelerometro com base nos 3 eixos
AccXangle = (atan2(ax, sqrt(pow(ay,2) + pow(az,2)))*180) / 3.14;
AccYangle = (atan2(ay, sqrt(pow(ax,2) + pow(az,2)))*180) / 3.14;
AccZangle = (atan2(az, sqrt(pow(ax,2) + pow(ay,2)))*180) / 3.14;
// Converte valor do giro em graus por seg
// multiplicando uma contante relacionada à taxa de amostragem do sensor
// nesse caso, a taxa é +-250g -> 0.00875
rate_gyr_x = gx*G_GAIN;
rate_gyr_y = gy*G_GAIN;
rate_gyr_z = gz*G_GAIN;
// Calcula a distância percorrida por integração simples
// com base no tempo de loop (dT = cT – pT)
gyroXangle+=rate_gyr_x*dT;
gyroYangle+=rate_gyr_y*dT;
gyroZangle+=rate_gyr_z*dT;
// Fusão dos dados: giro + accel
// Métodos: filtro complementar ou filtro de kalman
// Optei pelo Filtro Complementar por ser mais simples de se aplicar do que o Filtro de Kalman.
// Eficiencia bastante satisfatoria, segundo teoria
// Atribui peso de 0.98 ao valor do giro e 0.02 ao acelerometro
// O giroscópio tem leitura muito boa, mas também apresenta oscilação do valor.
// Acelerômetro, por outro lado, é muito ruidoso, mas o desvio é zero.
CFangleX=AA*(CFangleX+rate_gyr_x*(dT/1000000)) +(1 – AA) * AccXangle;
CFangleY=AA*(CFangleY+rate_gyr_y*(dT/1000000)) +(1 – AA) * AccYangle;
CFangleZ=AA*(CFangleZ+rate_gyr_z*(dT/1000000)) +(1 – AA) * AccZangle;
}
[/sourcecode]


Obrigado por me ajudar a entender, realmente me foi útil.
Muito boa explicação! Me ajudou bastante!
Estou configurando a MPU6050 e esta difícil encontrar material satisfatório!
Ótima explicação, tá de parabéns!! Você tocou em um ponto que estou atrás, mas não acho solução… Eu queria saber se existe algum meio de calcular a distância percorrida de forma linear isolada nos eixos x, y e z, mesmo se inclinar o sensor em algum ângulo, permanecer o sinal sem variação.
Olá!
Porque você ustilizou um ganho de 0,00875, se para a sensibilidade de 250 dps a folha de dados do fabricante pede um ganho de 0,131?
Fora esta dúvida agradeço pelo ótima explicação!
No datasheet do fabricante do sensor L3G4200D:
FS = 250 dps 8.75 mdps/digit
FS = 500 dps 17.50 mdps/digit
FS = 2000 dps 70 mdps/digit
Mas se fosse o MPU6050 o valor raw seria multiplicado por 0,131?
Olá! Para o MPU6050 usei a seguinte função:
void Le_MPU6050(void *pvParameters) {
uint8_t data[10];
while (1) {
if (xSemaphoreTake(MUTEX_BUS_I2C0, 1000 / portTICK_PERIOD_MS)) {
mpuReadfromReg(0x3B, data, 8);
int16_t RAWX = (data[0] << 8) | data[1];
int16_t RAWY = (data[2] << 8) | data[3];
int16_t RAWZ = (data[4] << 8) | data[5];
int16_t TEMP = (data[6] << 8) | data[7];
float xg = 9.81 * (float)RAWX / 16384;
float yg = 9.81 * (float)RAWY / 16384;
float zg = 9.81 * (float)RAWZ / 16384;
float tp = (float)TEMP / 340.00 + 36.53;
float AccXangle = (atan2(xg, sqrt(pow(yg, 2) + pow(zg, 2))) * 180) / 3.14;
float AccYangle = (atan2(yg, sqrt(pow(xg, 2) + pow(zg, 2))) * 180) / 3.14;
float AccZangle = (atan2(zg, sqrt(pow(xg, 2) + pow(yg, 2))) * 180) / 3.14;
ESP_LOGI(TAGMPU, "X=%.2f\tY=%.2f\tZ=%.2f\tT=%.2f", AccXangle, AccYangle,
AccZangle, tp);
xSemaphoreGive(MUTEX_BUS_I2C0);
} else {
ESP_LOGE(TAGI2C0, "NAO FOI POSSIVEL PEGAR O RECURSO I2C!");
}
vTaskDelay(100 / portTICK_PERIOD_MS);
}
}
Olá! Muito obrigado pelo material! Fiquei com uma dúvida. Na medição dos valores, quando vario um dos eixos do sensor, não há nenhuma mudança nas leituras. Porém, quando vario os outros dois eixos do sensor, obtenho valores complementares entre “x” e “y” num caso, e “x” e “z” em outro. É para acontecer isso mesmo?
Olá! Muito obrigado pelo material! Fiquei com uma dúvida. Na medição dos valores, quando vario um dos eixos do sensor, não há nenhuma mudança nas leituras. Porém, quando vario os outros dois eixos do sensor, obtenho valores complementares (soma igual a aproximadamente 90) entre “x” e “y” num caso, e “x” e “z” em outro. É para acontecer isso mesmo?