Aceleração linear usando quaternion do DMP: remoção de gravidade “3D correta” e movimento (com limites honestos)
Na seção anterior nós removemos a gravidade usando roll/pitch, o que funciona bem para muita coisa, mas ainda é uma aproximação. Quando você passa a ter quaternion vindo do DMP, dá para fazer a separação gravidade vs aceleração linear de forma mais consistente em 3D, porque você pode reconstruir o vetor gravidade no referencial do sensor diretamente do quaternion, sem depender de Euler (que é mais sensível a certas combinações de ângulos). Esse é exatamente o motivo de muita implementação DMP trabalhar com dmpGetQuaternion() e dmpGetGravity() antes de derivar yaw/pitch/roll.
A ideia é a mesma: o acelerômetro mede (aprox.) gravidade + aceleração linear. Só que agora você obtém a gravidade como um vetor (\mathbf{g}_{sens}) bem definido a partir do quaternion do DMP; em seguida você subtrai isso do acelerômetro (em g) e converte para m/s² se quiser.
1) Gravidade a partir do quaternion (igual ao que as libs fazem, mas sem esconder o cálculo)
Uma forma padrão de obter o vetor gravidade no frame do sensor (em “g”) a partir do quaternion \(q = (w,x,y,z)\) é:
\[
g_x = 2(xz – wy)
\]
\[
g_y = 2(wx + yz)
\]
\[
g_z = w^2 – x^2 – y^2 + z^2
\]
Esse vetor tem módulo aproximado de 1 (em repouso) e aponta “para baixo” no referencial do sensor.
A seguir, código em C para calcular isso e remover gravidade do acelerômetro. Eu vou assumir que você já leu o pacote do FIFO e extraiu o quaternion q e também tem o acelerômetro em g (se vier cru, primeiro converte usando o fator de escala como fizemos lá no começo).
#include <math.h>
#include <stdint.h>
typedef struct { float w, x, y, z; } Quaternion;
typedef struct {
float ax_g, ay_g, az_g; // aceleração medida (em g)
} accel_g_t;
typedef struct {
float gx_g, gy_g, gz_g; // gravidade estimada (em g)
float ax_lin_g, ay_lin_g, az_lin_g; // aceleração linear (em g)
float ax_lin_ms2, ay_lin_ms2, az_lin_ms2; // em m/s²
} linacc_out_t;
static void gravity_from_quaternion(const Quaternion *q, float *gx, float *gy, float *gz)
{
// Fórmulas padrão para obter o vetor gravidade no frame do sensor.
const float w = q->w, x = q->x, y = q->y, z = q->z;
*gx = 2.0f * (x*z - w*y);
*gy = 2.0f * (w*x + y*z);
*gz = (w*w - x*x - y*y + z*z);
}
void linear_accel_from_dmp_quaternion(const accel_g_t *a_meas,
const Quaternion *q,
linacc_out_t *out)
{
float gx, gy, gz;
gravity_from_quaternion(q, &gx, &gy, &gz);
out->gx_g = gx;
out->gy_g = gy;
out->gz_g = gz;
// remove gravidade
out->ax_lin_g = a_meas->ax_g - gx;
out->ay_lin_g = a_meas->ay_g - gy;
out->az_lin_g = a_meas->az_g - gz;
// g -> m/s²
const float g0 = 9.80665f;
out->ax_lin_ms2 = out->ax_lin_g * g0;
out->ay_lin_ms2 = out->ay_lin_g * g0;
out->az_lin_ms2 = out->az_lin_g * g0;
}
Se o seu pipeline estiver coerente, em repouso a aceleração linear tende a ficar perto de zero (com ruído). Em acelerações reais, você vê picos limpos e coerentes no eixo correspondente.
2) Integrando para “movimento” sem se enganar
Com a aceleração linear em m/s² você pode integrar para velocidade/posição exatamente como antes. O que muda é que, com DMP, a remoção de gravidade tende a ser mais estável, então a aceleração linear fica “menos contaminada” por erro de orientação. Mesmo assim, o problema matemático continua: integrar ruído e bias gera deriva inevitável.
O que dá para fazer de maneira correta e simples (sem prometer o impossível) é incluir um detector de “parado” e aplicar um ZUPT (Zero Velocity Update): quando você detecta que o sistema está parado, você força a velocidade para zero e impede que ela derive.
Um detector “honesto” usa duas condições ao mesmo tempo: o módulo da aceleração linear está pequeno e o giroscópio está perto de zero. Se você está usando DMP, ainda assim você pode ler o gyro cru (ou o gyro convertido) para essa checagem.
#include <stdbool.h>
#include <math.h>
typedef struct {
float vx, vy, vz;
float px, py, pz;
} motion_state_t;
static bool is_stationary(const linacc_out_t *lin,
float gx_dps, float gy_dps, float gz_dps,
float lin_acc_thr_ms2,
float gyro_thr_dps)
{
const float a = sqrtf(lin->ax_lin_ms2*lin->ax_lin_ms2 +
lin->ay_lin_ms2*lin->ay_lin_ms2 +
lin->az_lin_ms2*lin->az_lin_ms2);
const float g = sqrtf(gx_dps*gx_dps + gy_dps*gy_dps + gz_dps*gz_dps);
return (a < lin_acc_thr_ms2) && (g < gyro_thr_dps);
}
void integrate_motion_with_zupt(const linacc_out_t *lin,
float gx_dps, float gy_dps, float gz_dps,
float dt_s,
motion_state_t *m)
{
// Integra velocidade
m->vx += lin->ax_lin_ms2 * dt_s;
m->vy += lin->ay_lin_ms2 * dt_s;
m->vz += lin->az_lin_ms2 * dt_s;
// Se parado, zera velocidade (ZUPT)
if (is_stationary(lin, gx_dps, gy_dps, gz_dps,
/*lin_acc_thr*/0.35f, /*gyro_thr*/2.0f))
{
m->vx = 0.0f;
m->vy = 0.0f;
m->vz = 0.0f;
}
// Integra posição
m->px += m->vx * dt_s;
m->py += m->vy * dt_s;
m->pz += m->vz * dt_s;
}
Esse ZUPT não “resolve o mundo”, mas muda muito o jogo quando seu objeto tem momentos de repouso (pé no chão, parada em bancada, pausas). Em robôs bípedes, pedômetros, ferramentas manuais e sistemas que “param e andam”, isso reduz a deriva de forma brutal. Em objetos sempre em movimento (drone, carro), você precisa de outra referência externa (GPS/visão/UWB) se quiser posição confiável.
Boas práticas para MPU6050 + DMP (o que evita dor de cabeça de verdade)
O DMP funciona bem quando você trata o FIFO como um barramento de dados com taxa fixa. Se você lê menos do que ele produz, vai ter overflow; se você lê de forma irregular, vai ter jitter e eventualmente pacotes acumulados. A solução de engenharia é sincronizar a leitura por interrupção e sempre consumir pacotes completos.
Em vez de ficar “polling” do FIFO sem critério, o ideal é ligar a interrupção de “DMP data ready” (ou a interrupção de FIFO) no pino INT do MPU6050 e acordar sua tarefa/loop quando chegar dado novo. A cada interrupção, você lê o FIFO count, verifica overflow e então consome exatamente packetSize bytes. Se estiver acumulado mais de um pacote, muitas aplicações preferem descartar pacotes antigos e ficar com o mais recente para reduzir latência, desde que isso não prejudique seu controle.
Outro detalhe que separa projeto estável de projeto instável é calibrar offsets. Você já viu que, sem compensação, o giroscópio deriva e o acelerômetro pode ter offset. A rotina típica é: sensor parado, coletar amostras, estimar bias, gravar em constantes (ou em NVS/EEPROM) e aplicar na inicialização. Mesmo com DMP, isso melhora o comportamento em repouso e diminui drift. Se você mudar a montagem mecânica, alimentação ou temperatura de operação, refazer calibração costuma ser necessário.
Por fim, escolha de escalas e DLPF continua importando com DMP. Se você põe faixa muito alta (ex.: ±2000 °/s) sem necessidade, você perde resolução; se põe DLPF muito “aberto”, você injeta ruído; se põe muito “fechado”, você adiciona atraso. O ajuste correto depende do seu caso: um gimbal lento pede suavidade e pouco ruído, um objeto vibrando pede DLPF mais agressivo, um sistema de controle rápido precisa equilibrar atraso e ruído.
