Sensori IMU nello sport: perché capire i quaternioni non è negoziabile

Qualche mese fa stavo lavorando a un sensore per il velocity-based training: un dispositivo che si aggancia al bilanciere e dice all’allenatore a quale velocità l’atleta lo sta muovendo. Sembra semplice: monti un accelerometro sul bilanciere, leggi l’accelerazione, la integri e ottieni la velocità.

Poi, dopo circa cinque minuti di prove, il problema salta fuori: il bilanciere ruota. Durante uno squat, uno stacco, una distensione, non resta mai perfettamente in bolla. Si inclina in avanti, rotola leggermente nelle mani, si flette sotto carico. E quando succede, il sensore ruota con lui.

L’asse che il sensore misura non è più quello che ti interessa.

Un passo indietro: perché misurare l’accelerazione?

La velocità è l’integrale dell’accelerazione. Se sai quanto velocemente qualcosa sta accelerando in ogni istante e sommi questi valori nel tempo, ottieni la sua velocità.

Gli accelerometri sono economici, piccoli e precisi. Un chip IMU combinato con accelerometro e giroscopio costa pochi euro e sta su un’unghia. Lo fissi al bilanciere, lo campioni 100 volte al secondo e integri.

Ma integrare cosa, esattamente? L’accelerazione in quale direzione?

In un esercizio con bilanciere ci interessa una cosa sola: la velocità verticale. Il bilanciere sale e scende. La gravità tira in una direzione. L’atleta spinge contro la gravità. Il numero che ti dice se l’atleta è veloce o affaticato è la velocità verticale del bilanciere rispetto al pavimento.

Quindi la domanda è: come fai a sapere quale direzione è verticale?

Il problema: due assi che non coincidono

Quando il sensore è fissato perfettamente in piano sul bilanciere, il suo asse Z punta dritto verso l’alto. Leggere Z ti dà l’accelerazione verticale. Facile.

Ma se agganci un sensore a un bilanciere vero, non starà mai perfettamente in piano. Montalo con un minimo disallineamento, fallo cadere, spostalo tra una sessione e l’altra, e l’allineamento cambia. Peggio ancora: anche se parti perfettamente allineato, il bilanciere si muove durante l’alzata. Basta una rotazione di 20 gradi per cambiare l’asse che il sensore sta effettivamente misurando.

Il sensore ha il proprio sistema di riferimento: X, Y, Z fissati al suo corpo. Il mondo ha il proprio sistema di riferimento: X, Y, Z fissati alla stanza, con Z sempre rivolto verso il soffitto. Quando il sensore si inclina, il suo asse Z punta in una direzione diagonale nello spazio, né verso l’alto né di lato, da qualche parte in mezzo. E l’accelerazione che il sensore legge sull’asse Z è una combinazione di tutte e tre le direzioni del mondo.

Se integri quel numero, ottieni la velocità nella direzione sbagliata. Potresti leggere il bilanciere come se si muovesse lateralmente quando in realtà sta salendo. Potresti perdere del tutto il movimento verticale perché il sensore è perpendicolare alla gravità.

La soluzione: quaternioni e filtro di Madgwick

L’approccio ovvio: tracciare la rotazione del sensore usando pitch, roll e yaw, i tre angoli che descrivono come è orientato nello spazio. Poi invertire matematicamente la rotazione per recuperare l’accelerazione nel sistema di riferimento del mondo.

Funziona, finché il sensore non raggiunge un orientamento in cui il pitch è esattamente 90 gradi. A quel punto, pitch e yaw diventano lo stesso asse. Ruotare intorno a uno è geometricamente identico a ruotare intorno all’altro. Hai perso un grado di libertà. La matematica va in crisi.

Questo è il gimbal lock, e non è un bug del software. È una singolarità matematica che non si può correggere con una patch. Per un sensore su un bilanciere che potrebbe inclinarsi lateralmente durante uno stacco pesante o una distensione sopra la testa, è un problema reale.

Diagramma del gimbal lock: a sinistra, stato normale con tre assi di rotazione indipendenti (pitch, roll, yaw). A destra, pitch = 90°: pitch e yaw ora puntano nella stessa direzione, un grado di libertà è perso in modo permanente.

La soluzione è rappresentare l’orientamento con i quaternioni invece che con gli angoli di Eulero.

Un quaternione è composto da quattro numeri che codificano una rotazione come un singolo angolo attorno a un singolo asse. A differenza degli angoli di Eulero, non esiste nessuna combinazione di orientamenti in cui due assi collassino in uno solo. Il gimbal lock, semplicemente, non può verificarsi.

Ma conoscere il formato della rotazione non basta. Devi anche stimare, in ogni istante, come il sensore è effettivamente orientato. Questo è il compito del filtro di Madgwick: fonde giroscopio e accelerometro per produrre una nuova stima dell’orientamento ogni 10 ms, tracciando come il sensore si muove in tempo reale.

Il filtro produce un quaternione. Applichi il quaternione alla lettura grezza del sensore. Ottieni l’accelerazione verticale corretta. Integri. Hai la velocità.

Per i curiosi della matematica

Non ti interessano le equazioni? Vai direttamente a La logica in parole semplici: il diagramma qui sotto spiega l’intero approccio senza formule.

Quaternioni

Un quaternione è quattro numeri: (w, x, y, z). Rispettano sempre una regola:

w² + x² + y² + z² = 1

Questo vincolo mantiene il quaternione sulla superficie di una sfera unitaria a quattro dimensioni. Pensala come un punto su un globo, ma con una dimensione in più.

I quattro numeri codificano una rotazione come un angolo attorno a un asse:

w = cos(θ/2): il coseno di metà dell’angolo di rotazione
(x, y, z) = sin(θ/2) · û: l’asse di rotazione scalato per il seno di metà angolo

Una rotazione di 90° attorno all’asse verticale Z dà θ = 90°, quindi w = cos(45°) ≈ 0.707, z = sin(45°) ≈ 0.707, x = y = 0. Un quaternione, una rotazione univoca.

Spiegazione del quaternione: a sinistra, un diagramma 3D che mostra un asse di rotazione û e l'angolo θ che ruota il vettore v in v_world. A destra, il significato di ciascuno dei quattro numeri w, x, y, z con un esempio numerico concreto.

Per ruotare un vettore v dal sistema di riferimento del sensore a quello del mondo:

v_world = q · v · q⁻¹

Dove q⁻¹ è il coniugato del quaternione (inverti il segno di x, y, z). Questo si traduce nelle tre righe con i prodotti scalari che trovi nel codice qui sotto.

Il filtro di Madgwick

Il giroscopio e l’accelerometro falliscono in modi opposti.

Il giroscopio traccia le rotazioni veloci con precisione ed è immune alle vibrazioni. Ma deriva. Il suo piccolo errore costante si accumula in uno scostamento via via più ampio, che finisce per non avere più nulla a che fare con la realtà. Bastano pochi minuti di funzionamento perché la stima dell’orientamento diventi visibilmente sbagliata, anche se il sensore si è mosso pochissimo.

L’accelerometro sa sempre dove si trova la gravità. Quando il sensore è fermo, fornisce un riferimento assoluto per capire dove sia l’alto. Durante il movimento, però, capta anche l’accelerazione propria del bilanciere e diventa rumoroso. Ma in ogni istante di immobilità, tra una ripetizione e l’altra o in fondo a un’alzata, fornisce comunque una correzione.

Il filtro combina i due segnali in tre passaggi ogni 10 ms.

Passo 1, predizione dal giroscopio. La lettura del giroscopio (gx, gy, gz) viene trattata come un quaternione puro ω = (0, gx, gy, gz) e usata per predire come è cambiato l’orientamento dall’ultimo campione:

q̇_gyro = ½ × q ⊗ ω

Dove ⊗ è la moltiplicazione tra quaternioni e q è la stima attuale dell’orientamento. Veloce e preciso, ma accumula deriva.

Passo 2, correzione dall’accelerometro. La lettura dell’accelerometro viene normalizzata in un vettore unitario â. Se q fosse perfetto, ruotare il vettore di gravità del mondo (0, 0, 1) per q⁻¹ darebbe esattamente â. La differenza è l’errore. Il filtro calcola il gradiente di quell’errore rispetto a q: la direzione nello spazio dei quaternioni che riduce più rapidamente lo scarto:

q̇_∇ = ∇f(q, â)

Accurato, ma rumoroso durante il movimento, per questo viene applicato con cautela.

Passo 3, combinazione e integrazione. β controlla quanto peso viene dato alla correzione:

q_{t+1} = q_t + ( q̇_gyro − β × q̇_∇ ) × Δt

Il filtro poi normalizza il risultato per mantenere q sulla sfera unitaria.

β dinamico

Un β fisso funziona, ma un approccio più intelligente lo regola a ogni passo in base a cosa sta effettivamente rilevando l’accelerometro.

Quando il sensore è fermo, l’accelerometro legge esattamente 1g: gravità pura, correzione affidabile. Alza β per correggere la deriva più in fretta.

Quando il bilanciere si muove velocemente, l’accelerometro capta anche la sua inerzia. La lettura si allontana da 9.81 m/s². La correzione diventa fuorviante. Abbassa β: fidati di più del giroscopio.

Un’implementazione pratica:

float a_mag = sqrtf(ax*ax + ay*ay + az*az);
float deviation = fabsf(a_mag - 9.81f) / 9.81f;  // 0 = perfect 1g, 1 = completely off

// Smooth interpolation: high beta when still, low beta when moving
float beta = 0.01f + (0.08f - 0.01f) * expf(-6.0f * deviation);

A riposo (deviation ≈ 0), β ≈ 0.08: la deriva si corregge rapidamente. A metà alzata (deviation ≈ 0.4), β ≈ 0.02: il giroscopio domina e la traiettoria dell’alzata resta pulita. Il filtro si autoregola.

La logica in parole semplici

Il diagramma qui sotto mostra l’intero ciclo di aggiornamento. Nessuna notazione richiesta.

Ciclo di aggiornamento del filtro di Madgwick: dal quaternione iniziale, attraverso la predizione del giroscopio, la correzione dell'accelerometro, la ponderazione dinamica di beta, la combinazione e integrazione, la normalizzazione, fino all'output finale come velocità verticale.

Cos’è davvero un quaternione. Quattro numeri che descrivono esattamente come il sensore è ruotato nello spazio: un angolo attorno a un asse. Gli angoli di Eulero falliscono quando due assi collassano in uno, il gimbal lock di cui parlavamo prima; i quaternioni no, mai. L’immagine nella sezione matematica qui sopra lo mostra geometricamente, ma non serve seguire l’algebra per usare il risultato: il filtro ti dà il quaternione, tu lo applichi ai dati del sensore.

Come funziona il filtro. A ogni passo, due input si contendono il controllo della stima dell’orientamento.

Il giroscopio dice al filtro come è ruotato il sensore dall’ultimo campione. Se ci si fidasse solo di lui, la rotazione verrebbe tracciata con precisione, ma ha un piccolo errore costante che nel giro di qualche minuto si accumula in una deriva visibile.

L’accelerometro offre un contrappeso a quella deriva: sa sempre dove si trova la gravità. Quando il sensore è fermo, questo riferimento è pulito e affidabile. Quando il sensore si muove velocemente, invece, capta anche l’inerzia del bilanciere e diventa inaffidabile.

A decidere chi vince è β. E il β dinamico prende questa decisione da solo, ogni 10 millisecondi, controllando quanto la lettura dell’accelerometro si avvicina a 1g: se è vicina, si fida della correzione dell’accelerometro; se è lontana, si fida invece del giroscopio.

Il risultato è un nuovo quaternione ogni 10 ms. Applicalo alla lettura di accelerazione del sensore e otterrai sempre il vettore corretto nel sistema di riferimento del mondo, indipendentemente da come è orientato il bilanciere.

Cosa mostrano i dati

Il modo più chiaro per vedere il problema è calcolare la velocità integrando entrambe le versioni, quella grezza nel sistema di riferimento del sensore e quella corretta con il quaternione, e confrontare i risultati:

Velocità calcolata nel sistema di riferimento del sensore (grigio, resta vicino allo zero) contro velocità nel sistema di riferimento del mondo dopo la rotazione col quaternione (ambra, mostra correttamente il bilanciere che scende fino a 1.3 m/s). Senza rotazione, il sensore non vede affatto il movimento.

Il sensore in questo test era montato con un angolo significativo, circa 120 gradi rispetto alla verticale. La curva grigia si muove a malapena perché l’asse Z del sensore puntava quasi di lato: stava misurando una proiezione dell’accelerazione verticale vicina allo zero, quindi integrarla dà una velocità vicina allo zero indipendentemente da quanto velocemente si muovesse il bilanciere. La curva ambra traccia correttamente il bilanciere che scende fino a 1.3 m/s: lo stesso movimento, visto attraverso la rotazione col quaternione che riallinea Z alla vera verticale.

Il bilanciere si è mosso. Il sensore non lo sapeva.

Per una ripetizione completa, ecco come appare il segnale correttamente orientato: velocità a sinistra, accelerazione a destra:

Ripetizione reale di uno squat: velocità verticale. Arancione: eccentrica, bilanciere in discesa fino a −0.85 m/s. Ciano: concentrica, bilanciere in salita fino a +1.40 m/s. — Velocità (m/s)

Ripetizione reale di uno squat: accelerazione verticale. Oscillante durante l'eccentrica, picco netto a +6.31 m/s² nella spinta concentrica. — Accelerazione (m/s²)

La velocità parte da zero, scende in negativo mentre l’atleta abbassa il bilanciere (eccentrica, arancione), poi sale bruscamente in positivo mentre l’atleta lo spinge di nuovo verso l’alto (concentrica, ciano). Il picco concentrico di +1.40 m/s è il numero che un allenatore usa per monitorare la velocità del bilanciere e dedurre l’affaticamento. Vale la pena notare l’asimmetria: la discesa eccentrica è ampia e arrotondata, la salita concentrica è un picco stretto. L’atleta frena durante la discesa ed esplode durante la salita.

L’accelerazione racconta la stessa storia, ma in modo più rumoroso. L’oscillazione durante l’eccentrica è reale: l’atleta lotta contro l’inerzia e si adatta di continuo. I picchi netti alla transizione sono l’impulso della spinta seguito dalla decelerazione al lockout. Senza la rotazione col quaternione, entrambe le curve sarebbero quasi piatte indipendentemente da quanto velocemente si muovesse il bilanciere.

Il codice, passo dopo passo

Uno sketch Arduino minimale che implementa tutto questo. Ogni riga è commentata:

#include "MadgwickAHRS.h"  // the filter library

MadgwickAHRS filter;  // one filter instance for the whole session

void setup() {
  imu.begin();              // start the IMU
  filter.reset();           // set quaternion to identity (no rotation assumed)

  // Read the accelerometer while the sensor is perfectly still.
  // This aligns the filter's reference frame to gravity, so the world
  // frame is correct from the very first rep.
  float ax, ay, az;
  imu.readAccel(ax, ay, az);
  filter.alignToGravity(ax, ay, az);
}

void loop() {
  float gx, gy, gz;   // angular velocity in degrees/s (from gyroscope)
  float ax, ay, az;   // raw acceleration in m/s² (from accelerometer)
  imu.readGyro(gx, gy, gz);
  imu.readAccel(ax, ay, az);

  float dt = 0.01f;  // 10 ms between samples, 100 Hz

  // --- Madgwick filter update ---
  // Dynamic beta: trust the accelerometer more when the sensor is still (norm ≈ 1g),
  // trust the gyroscope more when the bar is moving fast (norm departs from 1g).
  float a_mag   = sqrtf(ax*ax + ay*ay + az*az);
  float deviation = fabsf(a_mag - 9.81f) / 9.81f;
  float beta    = 0.01f + 0.07f * expf(-6.0f * deviation);

  // Runs three steps internally: gyroscope prediction (q̇_gyro = ½ q ⊗ ω),
  // accelerometer correction (q̇_∇ = ∇f(q, â)), and blend (q += (q̇_gyro − β·q̇_∇)·Δt).
  // After this call, filter.q0–q3 describe how the sensor is oriented right now.
  filter.update(gx, gy, gz, ax, ay, az, dt, beta);

  // --- Rotate acceleration from sensor frame to world frame ---
  float q0 = filter.q0(), q1 = filter.q1(),
        q2 = filter.q2(), q3 = filter.q3();

  // Subtract gravity from the sensor reading (gravity = 9.81 m/s² along sensor Z)
  float linAccX = ax;
  float linAccY = ay;
  float linAccZ = az - 9.81f;  // remove the 1g contribution before rotating

  // Apply the quaternion rotation: sensor frame to world frame.
  // These three lines are the core of the whole approach.
  float worldAccX = (1 - 2*(q2*q2 + q3*q3)) * linAccX
                  + (2*(q1*q2 - q0*q3))      * linAccY
                  + (2*(q1*q3 + q0*q2))      * linAccZ;

  float worldAccZ = (2*(q1*q3 - q0*q2))      * linAccX
                  + (2*(q2*q3 + q0*q1))      * linAccY
                  + (1 - 2*(q1*q1 + q2*q2))  * linAccZ;

  // worldAccZ is now the true vertical acceleration in m/s²,
  // regardless of how the barbell is oriented.

  // --- Integrate acceleration to get vertical velocity ---
  // Trapezoidal rule: average the current and previous sample, multiply by time step.
  static float velZ = 0;
  static float prevAcc = 0;
  velZ += (worldAccZ + prevAcc) * 0.5f * dt;  // velocity accumulates each sample
  prevAcc = worldAccZ;

  // velZ is now vertical velocity in m/s.
  // Positive = moving up, negative = moving down.
}

La rotazione consiste in tre prodotti scalari, uno per ogni asse di output. Dai in ingresso il vettore nel sistema di riferimento del sensore e ottieni in uscita il vettore nel sistema di riferimento del mondo. Il filtro astrae l’aggiornamento del quaternione; la formula di rotazione non cambia mai, indipendentemente dall’orientamento.

Perché questo conta nella pratica

Il problema non è un caso raro. Ogni volta che monti un sensore su qualcosa che ruota, e ogni bilanciere ruota, leggere l’asse Z grezzo del sensore ti dà il numero sbagliato. L’errore cresce con l’angolo di inclinazione: con 10 gradi di scarto l’errore è del 2%, con 30 gradi del 13%, con 90 gradi stai misurando qualcosa di completamente ortogonale rispetto a quello che volevi misurare.

Il velocity-based training prende decisioni basate su piccole differenze. Un calo da 1.0 a 0.85 m/s può essere la soglia per interrompere una serie. Se la misura stessa ha un errore del 10-15% a seconda di come è messo il sensore, quelle soglie non significano nulla.

La rotazione col quaternione risolve il problema. Il filtro di Madgwick stima l’orientamento in modo continuo, così la rotazione è sempre aggiornata, anche mentre il bilanciere si muove. Il costo è qualche microsecondo di calcolo per campione e un parametro del filtro da calibrare una sola volta.

Il sensore non ha bisogno di sapere dove sia l’alto. Ci pensa il filtro.