Aceleração “do objeto” e movimento: separando gravidade da aceleração linear (e por que integrar vira deriva)
Até aqui você já tem roll e pitch razoavelmente estáveis (via filtro complementar). Isso é o que permite dar o próximo passo de forma coerente: estimar qual parte do acelerômetro é gravidade e subtrair do sinal medido para sobrar a aceleração linear (aquela que realmente vem do movimento do objeto). Sem essa separação, qualquer tentativa de “obter movimento” vai confundir inclinação com aceleração e o resultado vira uma bagunça.
A ideia física é simples: o acelerômetro mede a soma da gravidade projetada no sensor com as acelerações lineares do corpo. Em notação vetorial, no referencial do sensor:
\[
\mathbf{a}{meas} \approx \mathbf{g}{sens} + \mathbf{a}_{lin}
\]
Logo:
\[
\mathbf{a}{lin} \approx \mathbf{a}{meas} – \mathbf{g}_{sens}
\]
O ponto crítico é estimar \(\mathbf{g}_{sens}\). Como você já tem roll e pitch, dá para reconstruir as componentes da gravidade no referencial do sensor usando trigonometria.
Estimando o vetor gravidade no referencial do sensor a partir de roll/pitch
Assumindo a convenção mais comum (roll = rotação em torno de X, pitch = rotação em torno de Y) e tomando o módulo da gravidade como 1 g, uma aproximação prática muito usada (suficiente para separar gravidade em aplicações comuns) é:
\[
g_x = -\sin(\theta)
\]
\[
g_y = \sin(\phi)\cos(\theta)
\]
\[
g_z = \cos(\phi)\cos(\theta)
\]
onde \(\phi\) é o roll e \(\theta\) é o pitch, em radianos. Esses termos são exatamente as projeções do vetor “para baixo” (gravidade) nos eixos do sensor após aplicar inclinações.
Código em C: gravidade estimada e aceleração linear (em g e em m/s²)
Aqui eu assumo que você está atualizando angles (roll/pitch) com o filtro complementar da seção anterior, e que si.ax_g, si.ay_g, si.az_g já estão em g.
#include <math.h>
typedef struct {
float gx_g, gy_g, gz_g; // gravidade estimada no sensor (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; // aceleração linear (em m/s²)
} mpu6050_linacc_t;
static float deg2rad(float deg) {
return deg * ((float)M_PI / 180.0f);
}
/**
* @brief Estima gravidade no frame do sensor a partir de roll/pitch e separa aceleração linear.
*
* Observação: sem magnetômetro, yaw não entra aqui; para remover gravidade, roll/pitch bastam.
*/
void mpu6050_linear_accel_from_angles(const mpu6050_si_t *si,
const mpu6050_angles_t *angles,
mpu6050_linacc_t *out)
{
const float roll = deg2rad(angles->roll_deg);
const float pitch = deg2rad(angles->pitch_deg);
// Gravidade estimada (em g) projetada nos eixos do sensor
const float gx = -sinf(pitch);
const float gy = sinf(roll) * cosf(pitch);
const float gz = cosf(roll) * cosf(pitch);
out->gx_g = gx;
out->gy_g = gy;
out->gz_g = gz;
// Aceleração linear (medida - gravidade)
out->ax_lin_g = si->ax_g - gx;
out->ay_lin_g = si->ay_g - gy;
out->az_lin_g = si->az_g - gz;
// Converte para 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;
}
O que você deve ver em repouso, com o sensor parado e com roll/pitch bem estimados, é: ax_lin_g, ay_lin_g, az_lin_g ficando perto de zero (com algum ruído). Quando você desloca o objeto, essas componentes passam a representar aceleração linear real (por exemplo, uma “puxada” para frente aparece em X, uma lateral em Y, etc., dependendo de como o sensor está montado).
Movimento: integrar aceleração linear para velocidade e posição (e o “elefante na sala”)
Em teoria, dá para obter velocidade e posição integrando:
\[
\mathbf{v}(t+\Delta t) = \mathbf{v}(t) + \mathbf{a}_{lin}\Delta t
\]
\[
\mathbf{x}(t+\Delta t) = \mathbf{x}(t) + \mathbf{v}(t)\Delta t
\]
Na prática, em IMUs baratas, isso deriva rápido. O motivo é cruel e inevitável: qualquer offset pequeno na aceleração (mesmo 0.01 g) vira erro de velocidade que cresce linearmente e erro de posição que cresce quadraticamente. Por isso, “odometria inercial pura” costuma funcionar só por janelas curtas ou precisa de correções externas (GPS, visão, UWB, zero-velocity update em passos, contato no chão, etc.).
Mesmo assim, para demonstrar o método corretamente (e você poder medir o tamanho do erro na sua aplicação), segue um integrador simples, com um “truque” mínimo para reduzir lixo: um deadband (zona morta) para zerar acelerações muito pequenas (ruído), e um vazamento (leak) suave na velocidade para não acumular drift tão agressivamente quando o sistema “deveria estar parado”.
typedef struct {
float vx, vy, vz; // m/s
float px, py, pz; // m
} mpu6050_motion_t;
/**
* @brief Integra aceleração linear para velocidade e posição (demonstração).
*
* @param deadband_ms2 Acelerações menores que isso são tratadas como zero (reduz ruído).
* @param vel_leak 0..1 (ex.: 0.001 a 0.01). “Puxa” a velocidade lentamente para zero.
*/
void mpu6050_integrate_motion(const mpu6050_linacc_t *lin,
float dt_s,
float deadband_ms2,
float vel_leak,
mpu6050_motion_t *m)
{
float ax = lin->ax_lin_ms2;
float ay = lin->ay_lin_ms2;
float az = lin->az_lin_ms2;
// Deadband para reduzir integração de ruído
if (fabsf(ax) < deadband_ms2) ax = 0.0f;
if (fabsf(ay) < deadband_ms2) ay = 0.0f;
if (fabsf(az) < deadband_ms2) az = 0.0f;
// Integra velocidade
m->vx += ax * dt_s;
m->vy += ay * dt_s;
m->vz += az * dt_s;
// “Leak” (um amortecimento simples contra deriva quando parado)
// vel_leak pequeno: 0.001..0.01 típico para testes
m->vx *= (1.0f - vel_leak);
m->vy *= (1.0f - vel_leak);
m->vz *= (1.0f - vel_leak);
// Integra posição
m->px += m->vx * dt_s;
m->py += m->vy * dt_s;
m->pz += m->vz * dt_s;
}
Esse integrador é propositalmente “honesto”: ele mostra o pipeline completo e deixa evidente o comportamento de deriva. Em aplicações reais, você normalmente adiciona pelo menos uma estratégia de correção, como detectar “estado parado” (módulo de aceleração perto de 1 g e giroscópio perto de 0) e então travar a velocidade em zero, ou usar alguma referência externa.
Exemplo de loop completo (amarrando tudo)
Aqui está o esqueleto de como isso fica no seu loop: lê sensor, atualiza ângulo (filtro complementar), separa aceleração linear e integra movimento.
mpu6050_gyro_bias_t bias;
mpu6050_comp_filter_t filt = { .alpha = 0.98f };
mpu6050_angles_t angles = {0};
mpu6050_motion_t motion = {0};
uint32_t t_prev = micros32();
for (;;) {
const float dt = dt_from_micros(&t_prev);
mpu6050_raw_t raw;
mpu6050_si_t si;
if (!mpu6050_read_raw(&raw)) continue;
mpu6050_convert_to_si(&cfg, &raw, &si);
// Atualiza roll/pitch combinando gyro + accel
mpu6050_complementary_update(&si, &bias, dt, &filt, &angles);
// Separa gravidade e obtém aceleração linear
mpu6050_linacc_t lin;
mpu6050_linear_accel_from_angles(&si, &angles, &lin);
// Integra (demonstração)
mpu6050_integrate_motion(&lin, dt, /*deadband*/0.10f, /*vel_leak*/0.002f, &motion);
// Aqui você pode enviar angles e motion via UART/BLE, etc.
}
